From be915c8339500ce81ff2b9944ca5d1ecc6395455 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Fri, 18 Oct 2019 13:21:25 -0600 Subject: common: Implement newton sphere fit Add implementation of Newton's method for sphere fitting. BUG=b:137758297,chromium:1023858 TEST=Added new unit tests BRANCH=None Change-Id: Ic78ec4f8a8c2f57ddfa1d5220861bf5c06981ad8 Signed-off-by: Yuval Peress Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1869730 Reviewed-by: Jack Rosenthal --- common/build.mk | 2 +- common/newton_fit.c | 186 ++++++++++++++++++++++++++++++++++++++++++++++++++++ common/vec3.c | 9 +++ 3 files changed, 196 insertions(+), 1 deletion(-) create mode 100644 common/newton_fit.c (limited to 'common') diff --git a/common/build.mk b/common/build.mk index fc65725863..beb01a83a7 100644 --- a/common/build.mk +++ b/common/build.mk @@ -121,7 +121,7 @@ common-$(CONFIG_RWSIG)+=rwsig.o vboot/common.o common-$(CONFIG_RWSIG_TYPE_RWSIG)+=vboot/vb21_lib.o common-$(CONFIG_MATH_UTIL)+=math_util.o common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o kasa.o math_util.o \ - mat44.o vec3.o + mat44.o vec3.o newton_fit.o common-$(CONFIG_SHA1)+= sha1.o common-$(CONFIG_SHA256)+=sha256.o common-$(CONFIG_SOFTWARE_CLZ)+=clz.o diff --git a/common/newton_fit.c b/common/newton_fit.c new file mode 100644 index 0000000000..ae81a45f07 --- /dev/null +++ b/common/newton_fit.c @@ -0,0 +1,186 @@ +/* Copyright 2020 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +#include "common.h" +#include "console.h" +#include "newton_fit.h" +#include "math.h" +#include "math_util.h" +#include + +#define CPRINTS(fmt, args...) cprints(CC_MOTION_SENSE, fmt, ##args) + +static fp_t distance_squared(fpv3_t a, fpv3_t b) +{ + fpv3_t delta; + + fpv3_init(delta, a[X] - b[X], a[Y] - b[Y], a[Z] - b[Z]); + return fpv3_dot(delta, delta); +} + +static fp_t compute_error(struct newton_fit *fit, fpv3_t center) +{ + fp_t error = FLOAT_TO_FP(0.0f); + struct queue_iterator it; + struct newton_fit_orientation *_it; + + for (queue_begin(fit->orientations, &it); it.ptr != NULL; + queue_next(fit->orientations, &it)) { + fp_t e; + + _it = (struct newton_fit_orientation *)it.ptr; + e = FLOAT_TO_FP(1.0f) - + distance_squared(_it->orientation, center); + error += fp_mul(e, e); + } + + return error; +} + +static bool is_ready_to_compute(struct newton_fit *fit, bool prune) +{ + bool has_min_samples = true; + struct queue_iterator it; + struct newton_fit_orientation *_it; + + /* Not full, not ready to compute. */ + if (!queue_is_full(fit->orientations)) + return false; + + /* Inspect all the orientations. */ + for (queue_begin(fit->orientations, &it); it.ptr != NULL; + queue_next(fit->orientations, &it)) { + _it = (struct newton_fit_orientation *)it.ptr; + /* If an orientation has too few samples, flag that. */ + CPRINTS(" orientation %u/%u", _it->nsamples, + fit->min_orientation_samples); + if (_it->nsamples < fit->min_orientation_samples) { + has_min_samples = false; + break; + } + } + + /* If all orientations have the minimum samples, we're done and can + * compute the bias. + */ + if (has_min_samples) + return true; + + /* If we got here and prune is true, then we need to remove the oldest + * entry to make room for new orientations. + */ + if (prune) + queue_advance_head(fit->orientations, 1); + + return false; +} + +void newton_fit_reset(struct newton_fit *fit) +{ + queue_init(fit->orientations); +} + +bool newton_fit_accumulate(struct newton_fit *fit, fp_t x, fp_t y, fp_t z) +{ + struct queue_iterator it; + struct newton_fit_orientation *_it; + fpv3_t v, delta; + + fpv3_init(v, x, y, z); + + /* Check if we can merge this new data point with an existing + * orientation. + */ + for (queue_begin(fit->orientations, &it); it.ptr != NULL; + queue_next(fit->orientations, &it)) { + _it = (struct newton_fit_orientation *)it.ptr; + + fpv3_sub(delta, v, _it->orientation); + /* Skip entries that are too far away. */ + if (fpv3_dot(delta, delta) >= fit->nearness_threshold) + continue; + + /* Merge new data point with this orientation. */ + fpv3_scalar_mul(_it->orientation, + FLOAT_TO_FP(1.0f) - fit->new_pt_weight); + fpv3_scalar_mul(v, fit->new_pt_weight); + fpv3_add(_it->orientation, _it->orientation, v); + if (_it->nsamples < 0xff) + _it->nsamples++; + return is_ready_to_compute(fit, false); + } + + /* If queue isn't full. */ + if (!queue_is_full(fit->orientations)) { + struct newton_fit_orientation entry; + + entry.nsamples = 1; + fpv3_init(entry.orientation, x, y, z); + queue_add_unit(fit->orientations, &entry); + + return is_ready_to_compute(fit, false); + } + + return is_ready_to_compute(fit, true); +} + +void newton_fit_compute(struct newton_fit *fit, fpv3_t bias, fp_t *radius) +{ + struct queue_iterator it; + struct newton_fit_orientation *_it; + fpv3_t new_bias, offset, delta; + fp_t error, new_error; + uint32_t iteration = 0; + fp_t inv_orient_count; + + if (queue_is_empty(fit->orientations)) + return; + + inv_orient_count = fp_div(FLOAT_TO_FP(1.0f), + queue_count(fit->orientations)); + + memcpy(new_bias, bias, sizeof(fpv3_t)); + new_error = compute_error(fit, new_bias); + + do { + memcpy(bias, new_bias, sizeof(fpv3_t)); + error = new_error; + fpv3_zero(offset); + + for (queue_begin(fit->orientations, &it); it.ptr != NULL; + queue_next(fit->orientations, &it)) { + fp_t mag; + + _it = (struct newton_fit_orientation *)it.ptr; + + fpv3_sub(delta, _it->orientation, bias); + mag = fpv3_norm(delta); + fpv3_scalar_mul(delta, + fp_div(mag - FLOAT_TO_FP(1.0f), mag)); + fpv3_add(offset, offset, delta); + } + + fpv3_scalar_mul(offset, inv_orient_count); + fpv3_add(new_bias, bias, offset); + new_error = compute_error(fit, new_bias); + if (new_error > error) + memcpy(new_bias, bias, sizeof(fpv3_t)); + ++iteration; + } while (iteration < fit->max_iterations && new_error < error && + new_error > fit->error_threshold); + + memcpy(bias, new_bias, sizeof(fpv3_t)); + + if (radius) { + *radius = FLOAT_TO_FP(0.0f); + for (queue_begin(fit->orientations, &it); it.ptr != NULL; + queue_next(fit->orientations, &it)) { + _it = (struct newton_fit_orientation *)it.ptr; + fpv3_sub(delta, _it->orientation, bias); + *radius += fpv3_norm(delta); + } + *radius *= inv_orient_count; + } +} diff --git a/common/vec3.c b/common/vec3.c index 29dc22f922..dadf7715ff 100644 --- a/common/vec3.c +++ b/common/vec3.c @@ -9,6 +9,15 @@ #include "vec3.h" #include "util.h" +static fpv3_t zero_initialized_vector = { + FLOAT_TO_FP(0.0f), FLOAT_TO_FP(0.0f), FLOAT_TO_FP(0.0f) +}; + +void fpv3_zero(fpv3_t v) +{ + memcpy(v, zero_initialized_vector, sizeof(fpv3_t)); +} + void fpv3_init(fpv3_t v, fp_t x, fp_t y, fp_t z) { v[X] = x; -- cgit v1.2.1