1 /*
2  * Copyright (C) 2016 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include "calibration/sphere_fit/sphere_fit_calibration.h"
18 
19 #include <errno.h>
20 #include <stdarg.h>
21 #include <stdio.h>
22 #include <string.h>
23 
24 #ifdef SPHERE_FIT_DBG_ENABLED
25 #include "calibration/util/cal_log.h"
26 #endif  // SPHERE_FIT_DBG_ENABLED
27 
28 #include "common/math/mat.h"
29 #include "common/math/vec.h"
30 #include "chre/util/nanoapp/assert.h"
31 
32 // FORWARD DECLARATIONS
33 ///////////////////////////////////////////////////////////////////////////////
34 // Utility for converting solver state to a calibration data structure.
35 static void convertStateToCalStruct(const float x[SF_STATE_DIM],
36                                     struct ThreeAxisCalData *calstruct);
37 
38 static bool runCalibration(struct SphereFitCal *sphere_cal,
39                            const struct SphereFitData *data,
40                            uint64_t timestamp_nanos);
41 
42 #define MIN_VALID_DATA_NORM (1e-4f)
43 
44 // FUNCTION IMPLEMENTATIONS
45 //////////////////////////////////////////////////////////////////////////////
sphereFitInit(struct SphereFitCal * sphere_cal,const struct LmParams * lm_params,const size_t min_num_points_for_cal)46 void sphereFitInit(struct SphereFitCal *sphere_cal,
47                    const struct LmParams *lm_params,
48                    const size_t min_num_points_for_cal) {
49   CHRE_ASSERT_NOT_NULL(sphere_cal);
50   CHRE_ASSERT_NOT_NULL(lm_params);
51 
52   // Initialize LM solver.
53   lmSolverInit(&sphere_cal->lm_solver, lm_params,
54                &sphereFitResidAndJacobianFunc);
55 
56   // Reset other parameters.
57   sphereFitReset(sphere_cal);
58 
59   // Set num points for calibration, checking that it is above min.
60   if (min_num_points_for_cal < MIN_NUM_SPHERE_FIT_POINTS) {
61     sphere_cal->min_points_for_cal = MIN_NUM_SPHERE_FIT_POINTS;
62   } else {
63     sphere_cal->min_points_for_cal = min_num_points_for_cal;
64   }
65 }
66 
sphereFitReset(struct SphereFitCal * sphere_cal)67 void sphereFitReset(struct SphereFitCal *sphere_cal) {
68   CHRE_ASSERT_NOT_NULL(sphere_cal);
69 
70   // Set state to default (diagonal scale matrix and zero offset).
71   memset(&sphere_cal->x0[0], 0, sizeof(float) * SF_STATE_DIM);
72   sphere_cal->x0[eParamScaleMatrix11] = 1.f;
73   sphere_cal->x0[eParamScaleMatrix22] = 1.f;
74   sphere_cal->x0[eParamScaleMatrix33] = 1.f;
75   memcpy(sphere_cal->x, sphere_cal->x0, sizeof(sphere_cal->x));
76 
77   // Reset time.
78   sphere_cal->estimate_time_nanos = 0;
79 }
80 
sphereFitSetSolverData(struct SphereFitCal * sphere_cal,struct LmData * lm_data)81 void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
82                             struct LmData *lm_data) {
83   CHRE_ASSERT_NOT_NULL(sphere_cal);
84   CHRE_ASSERT_NOT_NULL(lm_data);
85 
86   // Set solver data.
87   lmSolverSetData(&sphere_cal->lm_solver, lm_data);
88 }
89 
sphereFitRunCal(struct SphereFitCal * sphere_cal,const struct SphereFitData * data,uint64_t timestamp_nanos)90 bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
91                      const struct SphereFitData *data,
92                      uint64_t timestamp_nanos) {
93   CHRE_ASSERT_NOT_NULL(sphere_cal);
94   CHRE_ASSERT_NOT_NULL(data);
95 
96   // Run calibration if have enough points.
97   if (data->num_fit_points >= sphere_cal->min_points_for_cal) {
98     return runCalibration(sphere_cal, data, timestamp_nanos);
99   }
100 
101   return false;
102 }
103 
sphereFitSetInitialBias(struct SphereFitCal * sphere_cal,const float initial_bias[THREE_AXIS_DIM])104 void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
105                              const float initial_bias[THREE_AXIS_DIM]) {
106   CHRE_ASSERT_NOT_NULL(sphere_cal);
107   sphere_cal->x0[eParamOffset1] = initial_bias[0];
108   sphere_cal->x0[eParamOffset2] = initial_bias[1];
109   sphere_cal->x0[eParamOffset3] = initial_bias[2];
110 }
111 
sphereFitGetLatestCal(const struct SphereFitCal * sphere_cal,struct ThreeAxisCalData * cal_data)112 void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal,
113                            struct ThreeAxisCalData *cal_data) {
114   CHRE_ASSERT_NOT_NULL(sphere_cal);
115   CHRE_ASSERT_NOT_NULL(cal_data);
116   convertStateToCalStruct(sphere_cal->x, cal_data);
117   cal_data->calibration_time_nanos = sphere_cal->estimate_time_nanos;
118 }
119 
sphereFitResidAndJacobianFunc(const float * state,const void * f_data,float * residual,float * jacobian)120 void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
121                                    float *residual, float *jacobian) {
122   CHRE_ASSERT_NOT_NULL(state);
123   CHRE_ASSERT_NOT_NULL(f_data);
124   CHRE_ASSERT_NOT_NULL(residual);
125 
126   const struct SphereFitData *data = (const struct SphereFitData *)f_data;
127 
128   // Verify that expected norm is non-zero, else use default of 1.0.
129   float expected_norm = 1.0;
130   CHRE_ASSERT(data->expected_norm > MIN_VALID_DATA_NORM);
131   if (data->expected_norm > MIN_VALID_DATA_NORM) {
132     expected_norm = data->expected_norm;
133   }
134 
135   // Convert parameters to calibration structure.
136   struct ThreeAxisCalData calstruct;
137   convertStateToCalStruct(state, &calstruct);
138 
139   // Compute Jacobian helper matrix if Jacobian requested.
140   //
141   // J = d(||M(x_data - bias)|| - expected_norm)/dstate
142   //   = (M(x_data - bias) / ||M(x_data - bias)||) * d(M(x_data - bias))/dstate
143   //   = x_corr / ||x_corr|| * A
144   // A = d(M(x_data - bias))/dstate
145   //   = [dy/dM11, dy/dM21, dy/dM22, dy/dM31, dy/dM32, dy/dM3,...
146   //      dy/db1, dy/db2, dy/db3]'
147   // where:
148   // dy/dM11 = [x_data[0] - bias[0], 0, 0]
149   // dy/dM21 = [0, x_data[0] - bias[0], 0]
150   // dy/dM22 = [0, x_data[1] - bias[1], 0]
151   // dy/dM31 = [0, 0, x_data[0] - bias[0]]
152   // dy/dM32 = [0, 0, x_data[1] - bias[1]]
153   // dy/dM33 = [0, 0, x_data[2] - bias[2]]
154   // dy/db1 = [-scale_factor_x, 0, 0]
155   // dy/db2 = [0, -scale_factor_y, 0]
156   // dy/db3 = [0, 0, -scale_factor_z]
157   float A[SF_STATE_DIM * THREE_AXIS_DIM];
158   if (jacobian) {
159     memset(jacobian, 0, sizeof(float) * SF_STATE_DIM * data->num_fit_points);
160     memset(A, 0, sizeof(A));
161     A[0 * SF_STATE_DIM + eParamOffset1] = -calstruct.scale_factor_x;
162     A[1 * SF_STATE_DIM + eParamOffset2] = -calstruct.scale_factor_y;
163     A[2 * SF_STATE_DIM + eParamOffset3] = -calstruct.scale_factor_z;
164   }
165 
166   // Loop over all data points to compute residual and Jacobian.
167   // TODO(dvitus): Use fit_data_std when available to weight residuals.
168   float x_corr[THREE_AXIS_DIM];
169   float x_bias_corr[THREE_AXIS_DIM];
170   size_t i;
171   for (i = 0; i < data->num_fit_points; ++i) {
172     const float *x_data = &data->fit_data[i * THREE_AXIS_DIM];
173 
174     // Compute corrected sensor data
175     calDataCorrectData(&calstruct, x_data, x_corr);
176 
177     // Compute norm of x_corr.
178     const float norm = vecNorm(x_corr, THREE_AXIS_DIM);
179 
180     // Compute residual error: f_x = norm - exp_norm
181     residual[i] = norm - data->expected_norm;
182 
183     // Compute Jacobian if valid pointer.
184     if (jacobian) {
185       if (norm < MIN_VALID_DATA_NORM) {
186         return;
187       }
188       const float scale = 1.f / norm;
189 
190       // Compute bias corrected data.
191       vecSub(x_bias_corr, x_data, calstruct.bias, THREE_AXIS_DIM);
192 
193       // Populate non-bias terms for A
194       A[0 + eParamScaleMatrix11] = x_bias_corr[0];
195       A[1 * SF_STATE_DIM + eParamScaleMatrix21] = x_bias_corr[0];
196       A[1 * SF_STATE_DIM + eParamScaleMatrix22] = x_bias_corr[1];
197       A[2 * SF_STATE_DIM + eParamScaleMatrix31] = x_bias_corr[0];
198       A[2 * SF_STATE_DIM + eParamScaleMatrix32] = x_bias_corr[1];
199       A[2 * SF_STATE_DIM + eParamScaleMatrix33] = x_bias_corr[2];
200 
201       // Compute J = x_corr / ||x_corr|| * A
202       matTransposeMultiplyVec(&jacobian[i * SF_STATE_DIM], A, x_corr,
203                               THREE_AXIS_DIM, SF_STATE_DIM);
204       vecScalarMulInPlace(&jacobian[i * SF_STATE_DIM], scale, SF_STATE_DIM);
205     }
206   }
207 }
208 
convertStateToCalStruct(const float x[SF_STATE_DIM],struct ThreeAxisCalData * calstruct)209 void convertStateToCalStruct(const float x[SF_STATE_DIM],
210                              struct ThreeAxisCalData *calstruct) {
211   memcpy(&calstruct->bias[0], &x[eParamOffset1],
212          sizeof(float) * THREE_AXIS_DIM);
213   calstruct->scale_factor_x = x[eParamScaleMatrix11];
214   calstruct->skew_yx = x[eParamScaleMatrix21];
215   calstruct->scale_factor_y = x[eParamScaleMatrix22];
216   calstruct->skew_zx = x[eParamScaleMatrix31];
217   calstruct->skew_zy = x[eParamScaleMatrix32];
218   calstruct->scale_factor_z = x[eParamScaleMatrix33];
219 }
220 
runCalibration(struct SphereFitCal * sphere_cal,const struct SphereFitData * data,uint64_t timestamp_nanos)221 bool runCalibration(struct SphereFitCal *sphere_cal,
222                     const struct SphereFitData *data,
223                     uint64_t timestamp_nanos) {
224   float x_sol[SF_STATE_DIM];
225 
226   // Run calibration
227   const enum LmStatus status =
228       lmSolverSolve(&sphere_cal->lm_solver, sphere_cal->x0, (void *)data,
229                     SF_STATE_DIM, data->num_fit_points, x_sol);
230 
231   // Check if solver was successful
232   if (status == RELATIVE_STEP_SUFFICIENTLY_SMALL ||
233       status == GRADIENT_SUFFICIENTLY_SMALL) {
234     // TODO(dvitus): Check quality of new fit before using.
235     // Store new fit.
236 #ifdef SPHERE_FIT_DBG_ENABLED
237     CAL_DEBUG_LOG("[SPHERE CAL]",
238                   "Solution found in %d iterations with status %d.\n",
239                   sphere_cal->lm_solver.num_iter, status);
240     CAL_DEBUG_LOG("[SPHERE CAL]", "Solution:\n {"
241                   CAL_FORMAT_6DIGITS " [M(1,1)], "
242                   CAL_FORMAT_6DIGITS " [M(2,1)], "
243                   CAL_FORMAT_6DIGITS " [M(2,2)], \n"
244                   CAL_FORMAT_6DIGITS " [M(3,1)], "
245                   CAL_FORMAT_6DIGITS " [M(3,2)], "
246                   CAL_FORMAT_6DIGITS " [M(3,3)], \n"
247                   CAL_FORMAT_6DIGITS " [b(1)], "
248                   CAL_FORMAT_6DIGITS " [b(2)], "
249                   CAL_FORMAT_6DIGITS " [b(3)]}.",
250                   CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6),
251                   CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6),
252                   CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6),
253                   CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6),
254                   CAL_ENCODE_FLOAT(x_sol[8], 6));
255 #endif  // SPHERE_FIT_DBG_ENABLED
256     memcpy(sphere_cal->x, x_sol, sizeof(x_sol));
257     sphere_cal->estimate_time_nanos = timestamp_nanos;
258     return true;
259   } else {
260 #ifdef SPHERE_FIT_DBG_ENABLED
261     CAL_DEBUG_LOG("[SPHERE CAL]",
262                   "Solution failed in %d iterations with status %d.\n",
263                   sphere_cal->lm_solver.num_iter, status);
264 #endif  // SPHERE_FIT_DBG_ENABLED
265   }
266 
267   return false;
268 }
269