12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added another test, fixed a bug in the factor w.r.t initializing measurement covariance.

release/4.3a0
krunalchande 2014-07-14 23:14:02 -04:00
parent b721a7ce1f
commit 73ec571f4b
2 changed files with 257 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/navigation/AHRSFactor.h
View File

@ -35,18 +35,19 @@ public:
const Matrix3& measuredOmegaCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration = false) : const bool use2ndOrderIntegration = false) :
biasHat(bias), measurementCovariance(9, 9), delRdelBiasOmega( biasHat(bias), measurementCovariance(3,3), delRdelBiasOmega(
Matrix3::Zero()), PreintMeasCov(9, 9), use2ndOrderIntegration_( Matrix3::Zero()), PreintMeasCov(3,3), use2ndOrderIntegration_(
use2ndOrderIntegration) { use2ndOrderIntegration) {
measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; // measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
PreintMeasCov = Matrix::Zero(9, 9); measurementCovariance <<measuredOmegaCovariance;
PreintMeasCov = Matrix::Zero(3,3);
} }
PreintegratedMeasurements() : PreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(9, 9), delRdelBiasOmega( biasHat(imuBias::ConstantBias()), measurementCovariance(3,3), delRdelBiasOmega(
Matrix3::Zero()), PreintMeasCov(9, 9) { Matrix3::Zero()), PreintMeasCov(3,3) {
measurementCovariance = Matrix::Zero(9, 9); measurementCovariance = Matrix::Zero(3,3);
PreintMeasCov = Matrix::Zero(9, 9); PreintMeasCov = Matrix::Zero(3,3);
} }
void print(const std::string& s = "Preintegrated Measurements: ") const { void print(const std::string& s = "Preintegrated Measurements: ") const {
@ -302,9 +303,10 @@ public:
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
H3->resize(3, 3); H3->resize(3, 6);
(*H3) << (*H3) <<
// dfR/dBias // dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega); Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega);
} }

324
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -35,38 +35,29 @@ using symbol_shorthand::X;
using symbol_shorthand::V; using symbol_shorthand::V;
using symbol_shorthand::B; using symbol_shorthand::B;
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
Vector callEvaluateError(const AHRSFactor& factor, Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
const Rot3 rot_i, const Rot3 rot_j, const Rot3 rot_j, const imuBias::ConstantBias& bias) {
const imuBias::ConstantBias& bias)
{
return factor.evaluateError(rot_i, rot_j, bias); return factor.evaluateError(rot_i, rot_j, bias);
} }
Rot3 evaluateRotationError(const AHRSFactor& factor, Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
const Rot3 rot_i, const Rot3 rot_j, const Rot3 rot_j, const imuBias::ConstantBias& bias) {
const imuBias::ConstantBias& bias) return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
{
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3) ) ;
} }
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
const list<Vector3>& measuredOmegas, const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
)
{
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity()); Matrix3::Identity(), Matrix3::Identity());
list<Vector3>::const_iterator itAcc = measuredAccs.begin(); list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin(); list<double>::const_iterator itDeltaT = deltaTs.begin();
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
} }
@ -74,53 +65,48 @@ AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
} }
Rot3 evaluatePreintegratedMeasurementsRotation( Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
const list<Vector3>& measuredOmegas, const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
const list<double>& deltaTs, return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) deltaTs, initialRotationRate).deltaRij;
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij;
} }
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
{ const double deltaT) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
} }
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
{
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AHRSFactor, PreintegratedMeasurements ) TEST( AHRSFactor, PreintegratedMeasurements ) {
{
// Linearization point // Linearization point
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
// Measurements // Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0); Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5; double deltaT = 0.5;
// Expected preintegrated values // Expected preintegrated values
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT1(0.5); double expectedDeltaT1(0.5);
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6)); EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6);
// Integrate again // Integrate again
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT2(1); double expectedDeltaT2(1);
// Actual preintegrated values // Actual preintegrated values
@ -132,30 +118,35 @@ TEST( AHRSFactor, PreintegratedMeasurements )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuFactor, Error ) TEST( ImuFactor, Error ) {
{
// Linearization point // Linearization point
imuBias::ConstantBias bias; // Bias imuBias::ConstantBias bias; // Bias
Rot3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0)); Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
Rot3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0)); Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; gravity << 0, 0, 9.81;
Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0;
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector(); Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector();
double deltaT = 1.0; double deltaT = 1.0;
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis, false); AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis,
false);
Vector errorActual = factor.evaluateError(x1, x2, bias); Vector errorActual = factor.evaluateError(x1, x2, bias);
// Expected error // Expected error
Vector errorExpected(3); errorExpected << 0, 0, 0; Vector errorExpected(3);
errorExpected << 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
@ -176,7 +167,6 @@ TEST( ImuFactor, Error )
Matrix H1a, H2a, H3a; Matrix H1a, H2a, H3a;
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
// rotations // rotations
EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
@ -185,13 +175,11 @@ TEST( ImuFactor, Error )
// rotations // rotations
EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
// EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions. EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions.
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiases ) TEST( ImuFactor, ErrorWithBiases ) {
{
// Linearization point // Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
@ -199,19 +187,24 @@ TEST( ImuFactor, ErrorWithBiases )
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0))); Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0))); Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; gravity << 0, 0, 9.81;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; Vector3 omegaCoriolis;
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); AHRSFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); // ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
@ -225,7 +218,8 @@ TEST( ImuFactor, ErrorWithBiases )
SETDEBUG("ImuFactor evaluateError", false); SETDEBUG("ImuFactor evaluateError", false);
// Expected error // Expected error
Vector errorExpected(3); errorExpected << 0, 0, 0; Vector errorExpected(3);
errorExpected << 0, 0, 0;
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
@ -237,11 +231,11 @@ TEST( ImuFactor, ErrorWithBiases )
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3,Rot3>( Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3,Rot3>( Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3,imuBias::ConstantBias>( Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians // Actual Jacobians
@ -250,28 +244,29 @@ TEST( ImuFactor, ErrorWithBiases )
EXPECT(assert_equal(H1e, H1a)); EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a)); EXPECT(assert_equal(H2e, H2a));
// EXPECT(assert_equal(H3e, H3a)); EXPECT(assert_equal(H3e, H3a));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AHRSFactor, PartialDerivativeExpmap ) TEST( AHRSFactor, PartialDerivativeExpmap ) {
{
// Linearization point // Linearization point
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
double deltaT = 0.5; double deltaT = 0.5;
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); boost::bind(&evaluateRotation, measuredOmega, _1, deltaT),
LieVector(biasOmega));
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
(measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
@ -279,24 +274,25 @@ TEST( AHRSFactor, PartialDerivativeExpmap )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AHRSFactor, PartialDerivativeLogmap ) TEST( AHRSFactor, PartialDerivativeLogmap ) {
{
// Linearization point // Linearization point
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias Vector3 thetahat;
thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 deltatheta; deltatheta << 0, 0, 0; Vector3 deltatheta;
deltatheta << 0, 0, 0;
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind( Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3) const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
double normx = norm_2(x); double normx = norm_2(x);
const Matrix3 actualDelFdeltheta = Matrix3::Identity() + const Matrix3 actualDelFdeltheta = Matrix3::Identity() + 0.5 * X
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
* X;
// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl; // std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl; // std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;
@ -307,29 +303,33 @@ TEST( AHRSFactor, PartialDerivativeLogmap )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AHRSFactor, fistOrderExponential ) TEST( AHRSFactor, fistOrderExponential ) {
{
// Linearization point // Linearization point
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
double deltaT = 1.0; double deltaT = 1.0;
// change w.r.t. linearization point // change w.r.t. linearization point
double alpha = 0.0; double alpha = 0.0;
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha;
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
(measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign const Matrix expectedRot = Rot3::Expmap(
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); const Matrix3 hatRot =
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); const Matrix3 actualRot = hatRot
const Matrix3 actualRot = * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
// Compare Jacobians // Compare Jacobians
@ -337,70 +337,79 @@ TEST( AHRSFactor, fistOrderExponential )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
{
// Linearization point // Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
// Measurements // Measurements
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs; list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
for(int i=1;i<100;i++) for (int i = 1; i < 100; i++) {
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
} }
// Actual preintegrated values // Actual preintegrated values
AHRSFactor::PreintegratedMeasurements preintegrated = AHRSFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0));
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>( Matrix expectedDelRdelBias =
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredAccs, measuredOmegas, deltaTs,
Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega,
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
} }
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
{
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0))); Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0))); Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; gravity << 0, 0, 9.81;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; Vector3 omegaCoriolis;
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), // ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega); // Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);
AHRSFactor::PreintegratedMeasurements pre_int_data(
AHRSFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -416,11 +425,11 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement )
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3,Rot3>( Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3,Rot3>( Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3,imuBias::ConstantBias>( Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians // Actual Jacobians
@ -429,11 +438,66 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement )
EXPECT(assert_equal(H1e, H1a)); EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a)); EXPECT(assert_equal(H2e, H2a));
// EXPECT(assert_equal(H3e, H3a)); EXPECT(assert_equal(H3e, H3a));
} }
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
TEST (AHRSFactor, graphTest) {
// linearization point
Rot3 x1(Rot3::RzRyRx(0, 0, 0));
Rot3 x2(Rot3::RzRyRx(0, M_PI/4, 0));
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
// PreIntegrator
imuBias::ConstantBias biasHat(Vector3(0, 0, 0), Vector3(0, 0, 0));
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
AHRSFactor::PreintegratedMeasurements pre_int_data(biasHat,
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
// Pre-integrate measurements
Vector3 measuredAcc(0.0, 0.0, 0.0);
Vector3 measuredOmega(0, M_PI/20, 0);
double deltaT = 1;
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create Factor
noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov);
// cout<<"model: \n"<<noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov)<<endl;
// cout<<"pre int measurement cov: \n "<<pre_int_data.PreintMeasCov<<endl;
NonlinearFactorGraph graph;
Values values;
for(size_t i = 0; i < 5; ++i) {
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}
// pre_int_data.print("Pre integrated measurementes");
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis);
values.insert(X(1), x1);
values.insert(X(2), x2);
values.insert(B(1), bias);
graph.push_back(factor);
// values.print();
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
// result.print("Final Result:\n");
// cout<<"******************************\n";
// cout<<"result.at(X(2)): \n"<<result.at<Rot3>(X(2)).ypr()*(180/M_PI);
Rot3 expectedRot(Rot3::RzRyRx(0, M_PI/4, 0));
EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
// Marginals marginals(graph, result);
// cout << "x1 covariance:\n" << marginals.marginalCovariance(X(1)) << endl;
// cout << "x2 covariance:\n" << marginals.marginalCovariance(X(2)) << endl;
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */