mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-18 08:54:15 +01:00
OP-1150 Add calibration util class implementing polynomial/ellipsoid fit algorithms
This commit is contained in:
Alessio Morale
parent
82691a9348
commit
46ff4c8bf9
@ -0,0 +1,294 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file calibrationutils.c
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
||||||
|
*
|
||||||
|
* @brief Utilities for calibration using ellipsoid fit algorithm
|
||||||
|
* @see The GNU Public License (GPL) Version 3
|
||||||
|
* @defgroup
|
||||||
|
* @{
|
||||||
|
*
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "calibrationutils.h"
|
||||||
|
using namespace Eigen;
|
||||||
|
namespace OpenPilot{
|
||||||
|
CalibrationUtils::CalibrationUtils()
|
||||||
|
{
|
||||||
|
NominalRange = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* The following code is based on RazorImu calibration samples that can be found here:
|
||||||
|
* https://github.com/ptrbrtz/razor-9dof-ahrs/tree/master/Matlab/magnetometer_calibration
|
||||||
|
*/
|
||||||
|
|
||||||
|
bool CalibrationUtils::Calibrate(){
|
||||||
|
Eigen::VectorXf samplesX = Eigen::VectorXf(samples_x);
|
||||||
|
Eigen::VectorXf samplesY = Eigen::VectorXf(samples_y);
|
||||||
|
Eigen::VectorXf samplesZ = Eigen::VectorXf(samples_z);
|
||||||
|
Eigen::VectorXf radii;
|
||||||
|
Eigen::Vector3f center;
|
||||||
|
Eigen::MatrixXf evecs;
|
||||||
|
|
||||||
|
this->EllipsoidFit(&samplesX, &samplesY, &samplesZ, ¢er, &radii, &evecs);
|
||||||
|
|
||||||
|
Scale.setZero();
|
||||||
|
|
||||||
|
Scale << NominalRange / radii.coeff(0),
|
||||||
|
NominalRange / radii.coeff(1),
|
||||||
|
NominalRange / radii.coeff(2);
|
||||||
|
|
||||||
|
Eigen::Matrix3f tmp;
|
||||||
|
tmp << Scale.coeff(0), 0, 0,
|
||||||
|
0, Scale.coeff(1), 0,
|
||||||
|
0, 0, Scale.coeff(2);
|
||||||
|
|
||||||
|
CalibrationMatrix = evecs * tmp * evecs.transpose();
|
||||||
|
Bias.setZero();
|
||||||
|
Bias << center.coeff(0), center.coeff(1), center.coeff(2);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* C++ Implementation of Yury Petrov's ellipsoid fit algorithm
|
||||||
|
* Following is the origial code and its license from which this implementation is derived
|
||||||
|
*
|
||||||
|
Copyright (c) 2009, Yury Petrov
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are
|
||||||
|
met:
|
||||||
|
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in
|
||||||
|
the documentation and/or other materials provided with the distribution
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||||
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||||
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||||
|
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||||
|
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||||
|
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||||
|
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||||
|
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||||
|
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
|
||||||
|
|
||||||
|
function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals )
|
||||||
|
%
|
||||||
|
% Fit an ellispoid/sphere to a set of xyz data points:
|
||||||
|
%
|
||||||
|
% [center, radii, evecs, pars ] = ellipsoid_fit( X )
|
||||||
|
% [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] );
|
||||||
|
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 );
|
||||||
|
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' );
|
||||||
|
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 );
|
||||||
|
%
|
||||||
|
% Parameters:
|
||||||
|
% * X, [x y z] - Cartesian data, n x 3 matrix or three n x 1 vectors
|
||||||
|
% * flag - 0 fits an arbitrary ellipsoid (default),
|
||||||
|
% - 1 fits an ellipsoid with its axes along [x y z] axes
|
||||||
|
% - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad
|
||||||
|
% - 3 fits a sphere
|
||||||
|
%
|
||||||
|
% Output:
|
||||||
|
% * center - ellispoid center coordinates [xc; yc; zc]
|
||||||
|
% * ax - ellipsoid radii [a; b; c]
|
||||||
|
% * evecs - ellipsoid radii directions as columns of the 3x3 matrix
|
||||||
|
% * v - the 9 parameters describing the ellipsoid algebraically:
|
||||||
|
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
|
||||||
|
%
|
||||||
|
% Author:
|
||||||
|
% Yury Petrov, Northeastern University, Boston, MA
|
||||||
|
%
|
||||||
|
|
||||||
|
error( nargchk( 1, 3, nargin ) ); % check input arguments
|
||||||
|
if nargin == 1
|
||||||
|
flag = 0; % default to a free ellipsoid
|
||||||
|
end
|
||||||
|
if flag == 2 && nargin == 2
|
||||||
|
equals = 'xy';
|
||||||
|
end
|
||||||
|
|
||||||
|
if size( X, 2 ) ~= 3
|
||||||
|
error( 'Input data must have three columns!' );
|
||||||
|
else
|
||||||
|
x = X( :, 1 );
|
||||||
|
y = X( :, 2 );
|
||||||
|
z = X( :, 3 );
|
||||||
|
end
|
||||||
|
|
||||||
|
% need nine or more data points
|
||||||
|
if length( x ) < 9 && flag == 0
|
||||||
|
error( 'Must have at least 9 points to fit a unique ellipsoid' );
|
||||||
|
end
|
||||||
|
if length( x ) < 6 && flag == 1
|
||||||
|
error( 'Must have at least 6 points to fit a unique oriented ellipsoid' );
|
||||||
|
end
|
||||||
|
if length( x ) < 5 && flag == 2
|
||||||
|
error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' );
|
||||||
|
end
|
||||||
|
if length( x ) < 3 && flag == 3
|
||||||
|
error( 'Must have at least 4 points to fit a unique sphere' );
|
||||||
|
end
|
||||||
|
|
||||||
|
if flag == 0
|
||||||
|
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
|
||||||
|
D = [ x .* x, ...
|
||||||
|
y .* y, ...
|
||||||
|
z .* z, ...
|
||||||
|
2 * x .* y, ...
|
||||||
|
2 * x .* z, ...
|
||||||
|
2 * y .* z, ...
|
||||||
|
2 * x, ...
|
||||||
|
2 * y, ...
|
||||||
|
2 * z ]; % ndatapoints x 9 ellipsoid parameters
|
||||||
|
elseif flag == 1
|
||||||
|
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
|
||||||
|
D = [ x .* x, ...
|
||||||
|
y .* y, ...
|
||||||
|
z .* z, ...
|
||||||
|
2 * x, ...
|
||||||
|
2 * y, ...
|
||||||
|
2 * z ]; % ndatapoints x 6 ellipsoid parameters
|
||||||
|
elseif flag == 2
|
||||||
|
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
|
||||||
|
% where A = B or B = C or A = C
|
||||||
|
if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
|
||||||
|
D = [ y .* y + z .* z, ...
|
||||||
|
x .* x, ...
|
||||||
|
2 * x, ...
|
||||||
|
2 * y, ...
|
||||||
|
2 * z ];
|
||||||
|
elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
|
||||||
|
D = [ x .* x + z .* z, ...
|
||||||
|
y .* y, ...
|
||||||
|
2 * x, ...
|
||||||
|
2 * y, ...
|
||||||
|
2 * z ];
|
||||||
|
else
|
||||||
|
D = [ x .* x + y .* y, ...
|
||||||
|
z .* z, ...
|
||||||
|
2 * x, ...
|
||||||
|
2 * y, ...
|
||||||
|
2 * z ];
|
||||||
|
end
|
||||||
|
else
|
||||||
|
% fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
|
||||||
|
D = [ x .* x + y .* y + z .* z, ...
|
||||||
|
2 * x, ...
|
||||||
|
2 * y, ...
|
||||||
|
2 * z ]; % ndatapoints x 4 sphere parameters
|
||||||
|
end
|
||||||
|
|
||||||
|
% solve the normal system of equations
|
||||||
|
v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
|
||||||
|
|
||||||
|
% find the ellipsoid parameters
|
||||||
|
if flag == 0
|
||||||
|
% form the algebraic form of the ellipsoid
|
||||||
|
A = [ v(1) v(4) v(5) v(7); ...
|
||||||
|
v(4) v(2) v(6) v(8); ...
|
||||||
|
v(5) v(6) v(3) v(9); ...
|
||||||
|
v(7) v(8) v(9) -1 ];
|
||||||
|
% find the center of the ellipsoid
|
||||||
|
center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
|
||||||
|
% form the corresponding translation matrix
|
||||||
|
T = eye( 4 );
|
||||||
|
T( 4, 1:3 ) = center';
|
||||||
|
% translate to the center
|
||||||
|
R = T * A * T';
|
||||||
|
% solve the eigenproblem
|
||||||
|
[ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
|
||||||
|
radii = sqrt( 1 ./ diag( evals ) );
|
||||||
|
else
|
||||||
|
if flag == 1
|
||||||
|
v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
|
||||||
|
elseif flag == 2
|
||||||
|
if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
|
||||||
|
v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||||
|
elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
|
||||||
|
v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||||
|
else % xy
|
||||||
|
v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
|
||||||
|
end
|
||||||
|
else
|
||||||
|
v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ];
|
||||||
|
end
|
||||||
|
center = ( -v( 7:9 ) ./ v( 1:3 ) )';
|
||||||
|
gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) );
|
||||||
|
radii = ( sqrt( gam ./ v( 1:3 ) ) )';
|
||||||
|
evecs = eye( 3 );
|
||||||
|
end
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
|
||||||
|
Eigen::Vector3f *center,
|
||||||
|
Eigen::VectorXf *radii,
|
||||||
|
Eigen::MatrixXf *evecs)
|
||||||
|
{
|
||||||
|
int numSamples = (*samplesX).rows();
|
||||||
|
Eigen::MatrixXf D(numSamples,9);
|
||||||
|
D.setZero();
|
||||||
|
D.col(0) = (*samplesX).cwiseProduct(*samplesX);
|
||||||
|
D.col(1) = (*samplesY).cwiseProduct(*samplesY);
|
||||||
|
D.col(2) = (*samplesZ).cwiseProduct(*samplesZ);
|
||||||
|
D.col(3) = (*samplesX).cwiseProduct(*samplesY) * 2;
|
||||||
|
D.col(4) = (*samplesX).cwiseProduct(*samplesZ) * 2;
|
||||||
|
D.col(5) = (*samplesY).cwiseProduct(*samplesZ) * 2;
|
||||||
|
D.col(6) = 2 * (*samplesX);
|
||||||
|
D.col(7) = 2 * (*samplesY);
|
||||||
|
D.col(8) = 2 * (*samplesZ);
|
||||||
|
|
||||||
|
Eigen::VectorXf ones(numSamples);
|
||||||
|
ones.setOnes(numSamples);
|
||||||
|
|
||||||
|
Eigen::MatrixXf dt1 = (D.transpose() * D);
|
||||||
|
Eigen::MatrixXf dt2 = (D.transpose() * ones);
|
||||||
|
Eigen::VectorXf v = dt1.inverse() * dt2;
|
||||||
|
|
||||||
|
Eigen::Matrix4f A;
|
||||||
|
A << v.coeff(0), v.coeff(3), v.coeff(4), v.coeff(6),
|
||||||
|
v.coeff(3), v.coeff(1), v.coeff(5), v.coeff(7),
|
||||||
|
v.coeff(4), v.coeff(5), v.coeff(2), v.coeff(8),
|
||||||
|
v.coeff(6), v.coeff(7), v.coeff(8), -1;
|
||||||
|
|
||||||
|
(*center) = -1 * A.block(0,0,3,3).inverse() * v.segment(6,3);
|
||||||
|
|
||||||
|
Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
|
||||||
|
t.block(3,0,1,3) = center->transpose();
|
||||||
|
|
||||||
|
Eigen::Matrix4f r = t * A * t.transpose();
|
||||||
|
|
||||||
|
Eigen::Matrix3f tmp2 = r.block(0,0,3,3) * -1 / r.coeff(3,3);
|
||||||
|
Eigen::EigenSolver<Eigen::Matrix3f> es(tmp2);
|
||||||
|
Eigen::VectorXf evals = es.eigenvalues().real();
|
||||||
|
(*evecs) = es.eigenvectors().real();
|
||||||
|
radii->setZero(3);
|
||||||
|
|
||||||
|
(*radii) = (evals.segment(0,3)).cwiseInverse().cwiseSqrt();
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,58 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
*
|
||||||
|
* @file calibrationutils.h
|
||||||
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
||||||
|
*
|
||||||
|
* @brief Utilities for calibration using ellipsoid fit algorithm
|
||||||
|
* @see The GNU Public License (GPL) Version 3
|
||||||
|
* @defgroup
|
||||||
|
* @{
|
||||||
|
*
|
||||||
|
*****************************************************************************/
|
||||||
|
/*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful, but
|
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||||
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||||
|
* for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef CALIBRATIONUTILS_H
|
||||||
|
#define CALIBRATIONUTILS_H
|
||||||
|
#include <Eigen/Core>
|
||||||
|
#include <Eigen/Eigenvalues>
|
||||||
|
#include <Eigen/Dense>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
namespace OpenPilot{
|
||||||
|
|
||||||
|
class CalibrationUtils
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
CalibrationUtils();
|
||||||
|
bool Calibrate();
|
||||||
|
Eigen::VectorXf samples_x;
|
||||||
|
Eigen::VectorXf samples_y;
|
||||||
|
Eigen::VectorXf samples_z;
|
||||||
|
Eigen::Matrix3f CalibrationMatrix;
|
||||||
|
Eigen::Vector3f Scale;
|
||||||
|
Eigen::Vector3f Bias;
|
||||||
|
float NominalRange;
|
||||||
|
private:
|
||||||
|
void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
|
||||||
|
Eigen::Vector3f *center,
|
||||||
|
Eigen::VectorXf *radii,
|
||||||
|
Eigen::MatrixXf *evecs);
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
#endif // CALIBRATIONUTILS_H
|
||||||
@ -39,7 +39,8 @@ HEADERS += configplugin.h \
|
|||||||
config_global.h \
|
config_global.h \
|
||||||
mixercurve.h \
|
mixercurve.h \
|
||||||
dblspindelegate.h \
|
dblspindelegate.h \
|
||||||
configrevohwwidget.h
|
configrevohwwidget.h \
|
||||||
|
calibration/calibrationutils.h \
|
||||||
|
|
||||||
SOURCES += configplugin.cpp \
|
SOURCES += configplugin.cpp \
|
||||||
configgadgetwidget.cpp \
|
configgadgetwidget.cpp \
|
||||||
@ -72,7 +73,8 @@ SOURCES += configplugin.cpp \
|
|||||||
outputchannelform.cpp \
|
outputchannelform.cpp \
|
||||||
mixercurve.cpp \
|
mixercurve.cpp \
|
||||||
dblspindelegate.cpp \
|
dblspindelegate.cpp \
|
||||||
configrevohwwidget.cpp
|
configrevohwwidget.cpp \
|
||||||
|
calibration/calibrationutils.cpp \
|
||||||
|
|
||||||
FORMS += airframe.ui \
|
FORMS += airframe.ui \
|
||||||
airframe_ccpm.ui \
|
airframe_ccpm.ui \
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user