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 <peress@chromium.org> Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1869730 Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
This commit is contained in:
Yuval Peress
Commit Bot
parent
b1743225b0
commit
be915c8339
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <string.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,117 @@
|
|||
/* 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.
|
||||
*/
|
||||
|
||||
/* Newton's method for sphere fit algorithm */
|
||||
|
||||
#ifndef __CROS_EC_NEWTON_FIT_H
|
||||
#define __CROS_EC_NEWTON_FIT_H
|
||||
|
||||
#include "queue.h"
|
||||
#include "vec3.h"
|
||||
#include "stdbool.h"
|
||||
|
||||
struct newton_fit_orientation {
|
||||
/** An orientations. */
|
||||
fpv3_t orientation;
|
||||
|
||||
/** The number of samples of this orientation. */
|
||||
uint8_t nsamples;
|
||||
};
|
||||
|
||||
struct newton_fit {
|
||||
/**
|
||||
* Threshold used to detect when two vectors are identical. Measured in
|
||||
* units^2.
|
||||
*/
|
||||
fp_t nearness_threshold;
|
||||
|
||||
/**
|
||||
* The weight to use for a new data point when computing the mean. When
|
||||
* a new point is considered the same as an existing orientation (via
|
||||
* the nearness_threshold) it will be averaged with the existing
|
||||
* orientation using this weight. Valid range is (0,1).
|
||||
*/
|
||||
fp_t new_pt_weight;
|
||||
|
||||
/**
|
||||
* The threshold used to determine whether or not to continue iterating
|
||||
* when performing the bias computation.
|
||||
*/
|
||||
fp_t error_threshold;
|
||||
|
||||
/**
|
||||
* The maximum number of orientations to use, changing this affects the
|
||||
* memory footprint of the algorithm as 3 floats are needed per
|
||||
* orientation.
|
||||
*/
|
||||
uint32_t max_orientations;
|
||||
|
||||
/**
|
||||
* The maximum number of iterations the algorithm is allowed to run.
|
||||
*/
|
||||
uint32_t max_iterations;
|
||||
|
||||
/**
|
||||
* The minimum number of samples per orientation to consider the
|
||||
* orientation ready for calculation
|
||||
*/
|
||||
uint8_t min_orientation_samples;
|
||||
|
||||
/**
|
||||
* Queue of newton_fit_orientation structs.
|
||||
*/
|
||||
struct queue *orientations;
|
||||
};
|
||||
|
||||
#define NEWTON_FIT(SIZE, NSAMPLES, NEAR_THRES, NEW_PT_WEIGHT, ERROR_THRESHOLD, \
|
||||
MAX_ITERATIONS) \
|
||||
((struct newton_fit){ \
|
||||
.nearness_threshold = NEAR_THRES, \
|
||||
.new_pt_weight = NEW_PT_WEIGHT, \
|
||||
.error_threshold = ERROR_THRESHOLD, \
|
||||
.max_orientations = SIZE, \
|
||||
.max_iterations = MAX_ITERATIONS, \
|
||||
.min_orientation_samples = NSAMPLES, \
|
||||
.orientations = (struct queue *)&QUEUE_NULL( \
|
||||
SIZE, struct newton_fit_orientation), \
|
||||
})
|
||||
|
||||
/**
|
||||
* Reset the newton_fit struct's state.
|
||||
*
|
||||
* @param fit Pointer to the struct.
|
||||
*/
|
||||
void newton_fit_reset(struct newton_fit *fit);
|
||||
|
||||
/**
|
||||
* Add new vector to the struct. The behavior of this depends on the
|
||||
* configuration values used when the struct was created. For example:
|
||||
* - Samples that are within sqrt(NEAR_THRES) of an existing orientation will
|
||||
* be averaged with the matching orientation entry.
|
||||
* - If the new sample isn't near an existing orientation it will only be added
|
||||
* if state->num_orientations < config->num_orientations.
|
||||
*
|
||||
* @param fit Pointer to the struct.
|
||||
* @param x The new samples' X component.
|
||||
* @param y The new samples' Y component.
|
||||
* @param z The new samples' Z component.
|
||||
* @return True if orientations are full and the struct is ready to compute the
|
||||
* bias.
|
||||
*/
|
||||
bool newton_fit_accumulate(struct newton_fit *fit, fp_t x, fp_t y, fp_t z);
|
||||
|
||||
/**
|
||||
* Compute the center/bias and optionally the radius represented by the current
|
||||
* struct.
|
||||
*
|
||||
* @param fit Pointer to the struct.
|
||||
* @param bias Pointer to the output bias (this is also the starting bias for
|
||||
* the algorithm.
|
||||
* @param radius Optional pointer to write the computed radius into. If NULL,
|
||||
* the calculation will be skipped.
|
||||
*/
|
||||
void newton_fit_compute(struct newton_fit *fit, fpv3_t bias, fp_t *radius);
|
||||
|
||||
#endif /* __CROS_EC_NEWTON_FIT_H */
|
||||
|
|
@ -12,6 +12,13 @@
|
|||
typedef float floatv3_t[3];
|
||||
typedef fp_t fpv3_t[3];
|
||||
|
||||
/**
|
||||
* Initialized a vector to all 0.0f.
|
||||
*
|
||||
* @param v Pointer to the vector that will be initialized.
|
||||
*/
|
||||
void fpv3_zero(fpv3_t v);
|
||||
|
||||
/**
|
||||
* Initialize a vector.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -52,6 +52,7 @@ test-list-host += motion_angle_tablet
|
|||
test-list-host += motion_lid
|
||||
test-list-host += motion_sense_fifo
|
||||
test-list-host += mutex
|
||||
test-list-host += newton_fit
|
||||
test-list-host += nvmem
|
||||
test-list-host += pingpong
|
||||
test-list-host += pinweaver
|
||||
|
|
@ -132,6 +133,7 @@ motion_lid-y=motion_lid.o
|
|||
motion_sense_fifo-y=motion_sense_fifo.o
|
||||
kasa-y=kasa.o
|
||||
mutex-y=mutex.o
|
||||
newton_fit-y=newton_fit.o
|
||||
nvmem-y=nvmem.o nvmem_tpm2_mock.o
|
||||
pingpong-y=pingpong.o
|
||||
pinweaver-y=pinweaver.o
|
||||
|
|
|
|||
|
|
@ -0,0 +1,140 @@
|
|||
/* 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 "newton_fit.h"
|
||||
#include "motion_sense.h"
|
||||
#include "test_util.h"
|
||||
#include <stdio.h>
|
||||
|
||||
/*
|
||||
* Need to define motion sensor globals just to compile.
|
||||
* We include motion task to force the inclusion of math_util.c
|
||||
*/
|
||||
struct motion_sensor_t motion_sensors[] = {};
|
||||
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
|
||||
|
||||
#define ACC(FIT, X, Y, Z, EXPECTED) \
|
||||
TEST_EQ(newton_fit_accumulate(FIT, X, Y, Z), EXPECTED, "%d")
|
||||
|
||||
static int test_newton_fit_reset(void)
|
||||
{
|
||||
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
|
||||
|
||||
newton_fit_reset(&fit);
|
||||
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
|
||||
TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
|
||||
newton_fit_reset(&fit);
|
||||
|
||||
TEST_EQ(queue_count(fit.orientations), (size_t)0, "%zu");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
static int test_newton_fit_accumulate(void)
|
||||
{
|
||||
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
|
||||
struct queue_iterator it;
|
||||
|
||||
newton_fit_reset(&fit);
|
||||
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
|
||||
|
||||
TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
|
||||
queue_begin(fit.orientations, &it);
|
||||
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
static int test_newton_fit_accumulate_merge(void)
|
||||
{
|
||||
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
|
||||
struct queue_iterator it;
|
||||
|
||||
newton_fit_reset(&fit);
|
||||
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
|
||||
newton_fit_accumulate(&fit, 1.05f, 0.0f, 0.0f);
|
||||
|
||||
TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
|
||||
queue_begin(fit.orientations, &it);
|
||||
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 2, "%u");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
static int test_newton_fit_accumulate_prune(void)
|
||||
{
|
||||
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
|
||||
struct queue_iterator it;
|
||||
|
||||
newton_fit_reset(&fit);
|
||||
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
|
||||
newton_fit_accumulate(&fit, -1.0f, 0.0f, 0.0f);
|
||||
newton_fit_accumulate(&fit, 0.0f, 1.0f, 0.0f);
|
||||
newton_fit_accumulate(&fit, 0.0f, -1.0f, 0.0f);
|
||||
|
||||
TEST_EQ(queue_is_full(fit.orientations), 1, "%d");
|
||||
queue_begin(fit.orientations, &it);
|
||||
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
|
||||
queue_next(fit.orientations, &it);
|
||||
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
|
||||
queue_next(fit.orientations, &it);
|
||||
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
|
||||
queue_next(fit.orientations, &it);
|
||||
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
|
||||
|
||||
newton_fit_accumulate(&fit, 0.0f, 0.0f, 1.0f);
|
||||
TEST_EQ(queue_is_full(fit.orientations), 0, "%d");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
static int test_newton_fit_calculate(void)
|
||||
{
|
||||
struct newton_fit fit = NEWTON_FIT(4, 3, 0.01f, 0.25f, 1.0e-8f, 100);
|
||||
floatv3_t bias;
|
||||
float radius;
|
||||
|
||||
newton_fit_reset(&fit);
|
||||
|
||||
ACC(&fit, 1.01f, 0.01f, 0.01f, false);
|
||||
ACC(&fit, 1.01f, 0.01f, 0.01f, false);
|
||||
ACC(&fit, 1.01f, 0.01f, 0.01f, false);
|
||||
|
||||
ACC(&fit, -0.99f, 0.01f, 0.01f, false);
|
||||
ACC(&fit, -0.99f, 0.01f, 0.01f, false);
|
||||
ACC(&fit, -0.99f, 0.01f, 0.01f, false);
|
||||
|
||||
ACC(&fit, 0.01f, 1.01f, 0.01f, false);
|
||||
ACC(&fit, 0.01f, 1.01f, 0.01f, false);
|
||||
ACC(&fit, 0.01f, 1.01f, 0.01f, false);
|
||||
|
||||
ACC(&fit, 0.01f, 0.01f, 1.01f, false);
|
||||
ACC(&fit, 0.01f, 0.01f, 1.01f, false);
|
||||
ACC(&fit, 0.01f, 0.01f, 1.01f, true);
|
||||
|
||||
fpv3_init(bias, 0.0f, 0.0f, 0.0f);
|
||||
newton_fit_compute(&fit, bias, &radius);
|
||||
|
||||
TEST_NEAR(bias[0], 0.01f, 0.0001f, "%f");
|
||||
TEST_NEAR(bias[1], 0.01f, 0.0001f, "%f");
|
||||
TEST_NEAR(bias[2], 0.01f, 0.0001f, "%f");
|
||||
TEST_NEAR(radius, 1.0f, 0.0001f, "%f");
|
||||
|
||||
return EC_SUCCESS;
|
||||
}
|
||||
|
||||
void run_test(void)
|
||||
{
|
||||
test_reset();
|
||||
|
||||
RUN_TEST(test_newton_fit_reset);
|
||||
RUN_TEST(test_newton_fit_accumulate);
|
||||
RUN_TEST(test_newton_fit_accumulate_merge);
|
||||
RUN_TEST(test_newton_fit_accumulate_prune);
|
||||
RUN_TEST(test_newton_fit_calculate);
|
||||
|
||||
test_print_result();
|
||||
}
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
/* Copyright 2019 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.
|
||||
*/
|
||||
|
||||
/**
|
||||
* See CONFIG_TASK_LIST in config.h for details.
|
||||
*/
|
||||
#define CONFIG_TEST_TASK_LIST /* No test task */
|
||||
|
|
@ -104,6 +104,11 @@
|
|||
#define CONFIG_ONLINE_CALIB
|
||||
#endif
|
||||
|
||||
#ifdef TEST_NEWTON_FIT
|
||||
#define CONFIG_FPU
|
||||
#define CONFIG_ONLINE_CALIB
|
||||
#endif
|
||||
|
||||
#if defined(TEST_MOTION_LID) || defined(TEST_MOTION_ANGLE) || \
|
||||
defined(TEST_MOTION_ANGLE_TABLET) || defined(TEST_MOTION_SENSE_FIFO)
|
||||
enum sensor_id {
|
||||
|
|
|
|||
Loading…
Reference in New Issue