ground/gcs/src/plugins/config/calibration/calibrationutils.h

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       calibrationutils.h
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
   6  *
   7  * @brief      Utilities for calibration. Ellipsoid and polynomial fit algorithms
   8  * @see        The GNU Public License (GPL) Version 3
   9  * @defgroup
  10  * @{
  11  *
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 #ifndef CALIBRATIONUTILS_H
  30 #define CALIBRATIONUTILS_H
  31 #include <Eigen/Core>
  32 #include <Eigen/Eigenvalues>
  33 #include <Eigen/Dense>
  34 #include <Eigen/LU>
  35 #include <QList>
  36 namespace OpenPilot {
  37 class CalibrationUtils {
  38 public:
  39     struct EllipsoidCalibrationResult {
  40         Eigen::Matrix3f CalibrationMatrix;
  41         Eigen::Vector3f Scale;
  42         Eigen::Vector3f Bias;
  43     };
  44     static bool EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
  45                                      float nominalRange,
  46                                      EllipsoidCalibrationResult *result,
  47                                      bool fitAlongXYZ);
  48     static bool PolynomialCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError);
  49
  50     static void ComputePoly(Eigen::VectorXf *samplesX, Eigen::VectorXf *polynomial, Eigen::VectorXf *polyY);
  51     static float ComputeSigma(Eigen::VectorXf *samplesY);
  52
  53     static int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
  54     static double listMean(QList<double> list);
  55     static double listVar(QList<double> list);
  56 private:
  57     static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
  58                              Eigen::Vector3f *center,
  59                              Eigen::VectorXf *radii,
  60                              Eigen::MatrixXf *evecs, bool fitAlongXYZ);
  61
  62     static int LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution);
  63 };
  64 }
  65 #endif // CALIBRATIONUTILS_H