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