ground/gcs/src/plugins/config/alignment-calibration.cpp

   1 #include "calibration.h"
   2 #include <Eigen/Core>
   3 #include <Eigen/Cholesky>
   4 #include <Eigen/Geometry>
   5
   6 using namespace Eigen;
   7
   8 /** Calibrate the angular misalignment of one field sensor relative to another.
   9  *
  10  * @param rotationVector[out] The rotation vector that rotates sensor 1 such
  11  * that its principle axes are colinear with the axes of sensor 0.
  12  * @param samples0[in] A list of samples of the field observed by the reference
  13  * sensor.
  14  * @param reference0[in] The common value of the reference field in the inertial
  15  * reference frame.
  16  * @param samples1[in] The list of samples taken by the sensor to be aligned to
  17  * the reference.  The attitude of the sensor head as a whole must be identical
  18  * between samples0[i] and samples1[i] for all i.
  19  * @param reference1[in] The actual value of the second field in the inertial
  20  * reference frame.
  21  * @param n_samples The number of samples.
  22  */
  23 void calibration_misalignment(Vector3f & rotationVector,
  24                               const Vector3f samples0[],
  25                               const Vector3f & reference0,
  26                               const Vector3f samples1[],
  27                               const Vector3f & reference1,
  28                               size_t n_samples)
  29 {
  30     // Note that this implementation makes the assumption that the angular
  31     // misalignment is small.  Something based on QUEST would be needed to
  32     // account for errors larger than a few degrees.
  33     Matrix<double, Dynamic, 3> X(n_samples, 3);
  34     Matrix<double, Dynamic, 1> y(n_samples, 1);
  35
  36     AngleAxisd reference(Quaterniond().setFromTwoVectors(
  37                              reference0.cast<double>(), reference1.cast<double>()));
  38     for (size_t i = 0; i < n_samples; ++i) {
  39         AngleAxisd observation(Quaterniond().setFromTwoVectors(
  40                                    samples0[i].cast<double>(), samples1[i].cast<double>()));
  41
  42         X.row(i) = observation.axis();
  43         y[i]     = reference.angle() - observation.angle();
  44     }
  45
  46     // Run linear least squares over the result.
  47     Vector3d result;
  48     (X.transpose() * X).ldlt().solve(X.transpose() * y, &result);
  49     rotationVector = result.cast<float>();
  50 }