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