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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       calibrationutils.c
   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 #include "calibrationutils.h"
  30 #include <math.h>
  31 using namespace Eigen;
  32 namespace OpenPilot {
  33 float CalibrationUtils::ComputeSigma(Eigen::VectorXf *samplesY)
  34 {
  35     Eigen::ArrayXd tmpd = samplesY->cast<double>().array();
  36     double mean = tmpd.mean();
  37
  38     return (float)sqrt((tmpd - mean).square().mean());
  39 }
  40
  41 /*
  42  * The following ellipsoid calibration code is based on RazorImu calibration samples that can be found here:
  43  * https://github.com/ptrbrtz/razor-9dof-ahrs/tree/master/Matlab/magnetometer_calibration
  44  */
  45 bool CalibrationUtils::EllipsoidCalibration(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
  46                                             float nominalRange,
  47                                             EllipsoidCalibrationResult *result,
  48                                             bool fitAlongXYZ)
  49 {
  50     Eigen::VectorXf radii;
  51     Eigen::Vector3f center;
  52     Eigen::MatrixXf evecs;
  53
  54     EllipsoidFit(samplesX, samplesY, samplesZ, &center, &radii, &evecs, fitAlongXYZ);
  55
  56     result->Scale.setZero();
  57
  58     result->Scale << nominalRange / radii.coeff(0),
  59         nominalRange / radii.coeff(1),
  60         nominalRange / radii.coeff(2);
  61
  62     Eigen::Matrix3f tmp;
  63     tmp << result->Scale.coeff(0), 0, 0,
  64         0, result->Scale.coeff(1), 0,
  65         0, 0, result->Scale.coeff(2);
  66
  67     result->CalibrationMatrix = evecs * tmp * evecs.transpose();
  68     result->Bias.setZero();
  69     result->Bias << center.coeff(0), center.coeff(1), center.coeff(2);
  70     return true;
  71 }
  72
  73 bool CalibrationUtils::PolynomialCalibration(VectorXf *samplesX, Eigen::VectorXf *samplesY, int degree, Eigen::Ref<Eigen::VectorXf> result, const double maxRelativeError)
  74 {
  75     int samples = samplesX->rows();
  76     // perform internal calculation using doubles
  77     VectorXd doubleX = samplesX->cast<double>();
  78     VectorXd doubleY = samplesY->cast<double>();
  79     Eigen::MatrixXd x(samples, degree + 1);
  80
  81     x.setOnes(samples, degree + 1);
  82
  83     for (int i = 1; i < degree + 1; i++) {
  84         Eigen::MatrixXd tmp  = Eigen::MatrixXd(x.col(i - 1));
  85         Eigen::MatrixXd tmp2 = tmp.cwiseProduct(doubleX);
  86
  87         x.col(i) = tmp2;
  88     }
  89     Eigen::MatrixXd xt   = x.transpose();
  90
  91     Eigen::MatrixXd xtx  = xt * x;
  92
  93     Eigen::VectorXd xty  = xt * doubleY;
  94     Eigen::VectorXd tmpx = xtx.fullPivHouseholderQr().solve(xty);
  95     result = tmpx.cast<float>();
  96     double relativeError = (xtx * tmpx - xty).norm() / xty.norm();
  97     return relativeError < maxRelativeError;
  98 }
  99
 100 void CalibrationUtils::ComputePoly(VectorXf *samplesX, Eigen::VectorXf *polynomial, VectorXf *polyY)
 101 {
 102     polyY->array().fill(polynomial->coeff(0));
 103     for (int i = 1; i < polynomial->rows(); i++) {
 104         polyY->array() += samplesX->array().pow(i) * polynomial->coeff(i);
 105     }
 106 }
 107
 108 /* C++ Implementation of Yury Petrov's ellipsoid fit algorithm
 109  * Following is the origial code and its license from which this implementation is derived
 110  *
 111    Copyright (c) 2009, Yury Petrov
 112    All rights reserved.
 113
 114    Redistribution and use in source and binary forms, with or without
 115    modification, are permitted provided that the following conditions are
 116    met:
 117
 118  * Redistributions of source code must retain the above copyright
 119       notice, this list of conditions and the following disclaimer.
 120  * Redistributions in binary form must reproduce the above copyright
 121       notice, this list of conditions and the following disclaimer in
 122       the documentation and/or other materials provided with the distribution
 123
 124    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 125    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 126    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 127    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 128    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 129    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 130    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 131    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 132    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 133    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 134    POSSIBILITY OF SUCH DAMAGE.
 135
 136
 137    function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals )
 138    %
 139    % Fit an ellispoid/sphere to a set of xyz data points:
 140    %
 141    %   [center, radii, evecs, pars ] = ellipsoid_fit( X )
 142    %   [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] );
 143    %   [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 );
 144    %   [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' );
 145    %   [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 );
 146    %
 147    % Parameters:
 148    % * X, [x y z]   - Cartesian data, n x 3 matrix or three n x 1 vectors
 149    % * flag         - 0 fits an arbitrary ellipsoid (default),
 150    %                - 1 fits an ellipsoid with its axes along [x y z] axes
 151    %                - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad
 152    %                - 3 fits a sphere
 153    %
 154    % Output:
 155    % * center    -  ellispoid center coordinates [xc; yc; zc]
 156    % * ax        -  ellipsoid radii [a; b; c]
 157    % * evecs     -  ellipsoid radii directions as columns of the 3x3 matrix
 158    % * v         -  the 9 parameters describing the ellipsoid algebraically:
 159    %                Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
 160    %
 161    % Author:
 162    % Yury Petrov, Northeastern University, Boston, MA
 163    %
 164
 165    error( nargchk( 1, 3, nargin ) );  % check input arguments
 166    if nargin == 1
 167     flag = 0;  % default to a free ellipsoid
 168    end
 169    if flag == 2 && nargin == 2
 170     equals = 'xy';
 171    end
 172
 173    if size( X, 2 ) ~= 3
 174     error( 'Input data must have three columns!' );
 175    else
 176     x = X( :, 1 );
 177     y = X( :, 2 );
 178     z = X( :, 3 );
 179    end
 180
 181    % need nine or more data points
 182    if length( x ) < 9 && flag == 0
 183    error( 'Must have at least 9 points to fit a unique ellipsoid' );
 184    end
 185    if length( x ) < 6 && flag == 1
 186    error( 'Must have at least 6 points to fit a unique oriented ellipsoid' );
 187    end
 188    if length( x ) < 5 && flag == 2
 189    error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' );
 190    end
 191    if length( x ) < 3 && flag == 3
 192    error( 'Must have at least 4 points to fit a unique sphere' );
 193    end
 194
 195    if flag == 0
 196     % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
 197     D = [ x .* x, ...
 198           y .* y, ...
 199           z .* z, ...
 200       2 * x .* y, ...
 201       2 * x .* z, ...
 202       2 * y .* z, ...
 203       2 * x, ...
 204       2 * y, ...
 205       2 * z ];  % ndatapoints x 9 ellipsoid parameters
 206    elseif flag == 1
 207     % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
 208     D = [ x .* x, ...
 209           y .* y, ...
 210           z .* z, ...
 211       2 * x, ...
 212       2 * y, ...
 213       2 * z ];  % ndatapoints x 6 ellipsoid parameters
 214    elseif flag == 2
 215     % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
 216     % where A = B or B = C or A = C
 217     if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
 218         D = [ y .* y + z .* z, ...
 219             x .* x, ...
 220             2 * x, ...
 221             2 * y, ...
 222             2 * z ];
 223     elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
 224         D = [ x .* x + z .* z, ...
 225             y .* y, ...
 226             2 * x, ...
 227             2 * y, ...
 228             2 * z ];
 229     else
 230         D = [ x .* x + y .* y, ...
 231             z .* z, ...
 232             2 * x, ...
 233             2 * y, ...
 234             2 * z ];
 235     end
 236    else
 237     % fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
 238     D = [ x .* x + y .* y + z .* z, ...
 239       2 * x, ...
 240       2 * y, ...
 241       2 * z ];  % ndatapoints x 4 sphere parameters
 242    end
 243
 244    % solve the normal system of equations
 245    v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
 246
 247    % find the ellipsoid parameters
 248    if flag == 0
 249     % form the algebraic form of the ellipsoid
 250     A = [ v(1) v(4) v(5) v(7); ...
 251           v(4) v(2) v(6) v(8); ...
 252           v(5) v(6) v(3) v(9); ...
 253           v(7) v(8) v(9) -1 ];
 254     % find the center of the ellipsoid
 255     center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
 256     % form the corresponding translation matrix
 257     T = eye( 4 );
 258     T( 4, 1:3 ) = center';
 259     % translate to the center
 260     R = T * A * T';
 261     % solve the eigenproblem
 262     [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
 263     radii = sqrt( 1 ./ diag( evals ) );
 264    else
 265     if flag == 1
 266         v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
 267     elseif flag == 2
 268         if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
 269             v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
 270         elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
 271             v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
 272         else % xy
 273             v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
 274         end
 275     else
 276         v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ];
 277     end
 278     center = ( -v( 7:9 ) ./ v( 1:3 ) )';
 279     gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) );
 280     radii = ( sqrt( gam ./ v( 1:3 ) ) )';
 281     evecs = eye( 3 );
 282    end
 283
 284  */
 285
 286 void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
 287                                     Eigen::Vector3f *center,
 288                                     Eigen::VectorXf *radii,
 289                                     Eigen::MatrixXf *evecs,
 290                                     bool fitAlongXYZ)
 291 {
 292     int numSamples = (*samplesX).rows();
 293     Eigen::MatrixXf D;
 294
 295     if (!fitAlongXYZ) {
 296         D.setZero(numSamples, 9);
 297         D.col(0) = (*samplesX).cwiseProduct(*samplesX);
 298         D.col(1) = (*samplesY).cwiseProduct(*samplesY);
 299         D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
 300         D.col(3) = (*samplesX).cwiseProduct(*samplesY) * 2;
 301         D.col(4) = (*samplesX).cwiseProduct(*samplesZ) * 2;
 302         D.col(5) = (*samplesY).cwiseProduct(*samplesZ) * 2;
 303         D.col(6) = 2 * (*samplesX);
 304         D.col(7) = 2 * (*samplesY);
 305         D.col(8) = 2 * (*samplesZ);
 306     } else {
 307         D.setZero(numSamples, 6);
 308         D.setZero();
 309         D.col(0) = (*samplesX).cwiseProduct(*samplesX);
 310         D.col(1) = (*samplesY).cwiseProduct(*samplesY);
 311         D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
 312         D.col(3) = 2 * (*samplesX);
 313         D.col(4) = 2 * (*samplesY);
 314         D.col(5) = 2 * (*samplesZ);
 315     }
 316     Eigen::VectorXf ones(numSamples);
 317     ones.setOnes(numSamples);
 318
 319     Eigen::MatrixXf dt1 = (D.transpose() * D);
 320     Eigen::MatrixXf dt2 = (D.transpose() * ones);
 321     Eigen::VectorXf v   = dt1.inverse() * dt2;
 322
 323     if (!fitAlongXYZ) {
 324         Eigen::Matrix4f A;
 325         A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6),
 326             v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7),
 327             v.coeff(4), v.coeff(5), v.coeff(2), v.coeff(8),
 328             v.coeff(6), v.coeff(7), v.coeff(8), -1;
 329
 330         (*center) = -1 * A.block(0, 0, 3, 3).inverse() * v.segment(6, 3);
 331
 332         Eigen::Matrix4f t     = Eigen::Matrix4f::Identity();
 333         t.block(3, 0, 1, 3) = center->transpose();
 334
 335         Eigen::Matrix4f r     = t * A * t.transpose();
 336
 337         Eigen::Matrix3f tmp2  = r.block(0, 0, 3, 3) * -1 / r.coeff(3, 3);
 338         Eigen::EigenSolver<Eigen::Matrix3f> es(tmp2);
 339         Eigen::VectorXf evals = es.eigenvalues().real();
 340
 341         (*evecs) = es.eigenvectors().real();
 342         radii->setZero(3);
 343         (*radii) = (evals.segment(0, 3)).cwiseInverse().cwiseSqrt();
 344     } else {
 345         Eigen::VectorXf v1(9);
 346
 347         v1 << v.coeff(0), v.coeff(1), v.coeff(2), 0.0f, 0.0f, 0.0f, v.coeff(3), v.coeff(4), v.coeff(5);
 348         (*center) = (-1) * v1.segment(6, 3).cwiseProduct(v1.segment(0, 3).cwiseInverse());
 349
 350         float gam = 1 + (v1.coeff(6) * v1.coeff(6) / v1.coeff(0) +
 351                          v1.coeff(7) * v1.coeff(7) / v1.coeff(1) +
 352                          v1.coeff(8) * v1.coeff(8) / v1.coeff(2));
 353         radii->setZero(3);
 354         (*radii) = (v1.segment(0, 3).cwiseInverse() * gam).cwiseSqrt();
 355         evecs->setIdentity(3, 3);
 356     }
 357 }
 358
 359 int CalibrationUtils::SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3])
 360 {
 361     int i;
 362     double A[5][5];
 363     double f[5], c[5];
 364     double xp, yp, zp, Sx;
 365
 366     // Fill in matrix A -
 367     // write six difference-in-magnitude equations of the form
 368     // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
 369     // or in other words
 370     // 2*Sx*bx*(x2-x1)/Sx^2  + Sy^2(y2^2-y1^2)/Sx^2  + 2*Sy*by*(y2-y1)/Sx^2  + Sz^2(z2^2-z1^2)/Sx^2  + 2*Sz*bz*(z2-z1)/Sx^2  = (x1^2-x2^2)
 371     for (i = 0; i < 5; i++) {
 372         A[i][0] = 2.0 * (x[i + 1] - x[i]);
 373         A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i];
 374         A[i][2] = 2.0 * (y[i + 1] - y[i]);
 375         A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i];
 376         A[i][4] = 2.0 * (z[i + 1] - z[i]);
 377         f[i]    = x[i] * x[i] - x[i + 1] * x[i + 1];
 378     }
 379
 380     // solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
 381     if (!LinearEquationsSolve(5, (double *)A, f, c)) {
 382         return 0;
 383     }
 384
 385     // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
 386     xp   = x[0]; yp = y[0]; zp = z[0];
 387     Sx   = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3]));
 388
 389     S[0] = Sx;
 390     b[0] = Sx * c[0];
 391     S[1] = sqrt(c[1] * Sx * Sx);
 392     b[1] = c[2] * Sx * Sx / S[1];
 393     S[2] = sqrt(c[3] * Sx * Sx);
 394     b[2] = c[4] * Sx * Sx / S[2];
 395
 396     return 1;
 397 }
 398
 399
 400 int CalibrationUtils::LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution)
 401 {
 402     double fMaxElem;
 403     double fAcc;
 404
 405     int i, j, k, m;
 406
 407     for (k = 0; k < (nDim - 1); k++) { // base row of matrix
 408         // search of line with max element
 409         fMaxElem = fabs(pfMatr[k * nDim + k]);
 410         m = k;
 411         for (i = k + 1; i < nDim; i++) {
 412             if (fMaxElem < fabs(pfMatr[i * nDim + k])) {
 413                 fMaxElem = pfMatr[i * nDim + k];
 414                 m = i;
 415             }
 416         }
 417
 418         // permutation of base line (index k) and max element line(index m)
 419         if (m != k) {
 420             for (i = k; i < nDim; i++) {
 421                 fAcc = pfMatr[k * nDim + i];
 422                 pfMatr[k * nDim + i] = pfMatr[m * nDim + i];
 423                 pfMatr[m * nDim + i] = fAcc;
 424             }
 425             fAcc = pfVect[k];
 426             pfVect[k] = pfVect[m];
 427             pfVect[m] = fAcc;
 428         }
 429
 430         if (pfMatr[k * nDim + k] == 0.) {
 431             return 0; // needs improvement !!!
 432         }
 433         // triangulation of matrix with coefficients
 434         for (j = (k + 1); j < nDim; j++) { // current row of matrix
 435             fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
 436             for (i = k; i < nDim; i++) {
 437                 pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i];
 438             }
 439             pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
 440         }
 441     }
 442
 443     for (k = (nDim - 1); k >= 0; k--) {
 444         pfSolution[k] = pfVect[k];
 445         for (i = (k + 1); i < nDim; i++) {
 446             pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]);
 447         }
 448         pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
 449     }
 450
 451     return 1;
 452 }
 453
 454 double CalibrationUtils::listMean(QList<double> list)
 455 {
 456     double accum = 0;
 457
 458     for (int i = 0; i < list.size(); i++) {
 459         accum += list[i];
 460     }
 461     return accum / list.size();
 462 }
 463
 464 double CalibrationUtils::listVar(QList<double> list)
 465 {
 466     double mean_accum = 0;
 467     double var_accum  = 0;
 468     double mean;
 469
 470     for (int i = 0; i < list.size(); i++) {
 471         mean_accum += list[i];
 472     }
 473     mean = mean_accum / list.size();
 474
 475     for (int i = 0; i < list.size(); i++) {
 476         var_accum += (list[i] - mean) * (list[i] - mean);
 477     }
 478
 479     // Use unbiased estimator
 480     return var_accum / (list.size() - 1);
 481 }
 482 }