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

Re-formatted to BORG-style

release/4.3a0
dellaert 2015-07-15 23:24:24 -07:00
parent 0c3bb85547
commit e7c2e81831
1 changed files with 218 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

387
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -43,19 +43,20 @@ static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
namespace { namespace {
// Auxiliary functions to test evaluate error in ImuFactor // Auxiliary functions to test evaluate error in ImuFactor
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
const Pose3& pose_j, const Vector3& vel_j, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) { const imuBias::ConstantBias& bias) {
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3)); return Rot3::Expmap(
factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3));
} }
// Auxiliary functions to test Jacobians F and G used for // Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration // covariance propagation during preintegration
/* ************************************************************************* */ /* ************************************************************************* */
Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, const Vector3& deltaVij_old, Vector updatePreintegratedPosVel(const Vector3 deltaPij_old,
const Rot3& deltaRij_old, const Vector3& correctedAcc, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedOmega, const double deltaT, const Vector3& correctedAcc, const Vector3& correctedOmega,
const bool use2ndOrderIntegration_) { const double deltaT, const bool use2ndOrderIntegration_) {
Matrix3 dRij = deltaRij_old.matrix(); Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * correctedAcc * deltaT;
Vector3 deltaPij_new; Vector3 deltaPij_new;
@ -71,8 +72,8 @@ Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, const Vector3& delt
return result; return result;
} }
Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, const Vector3& correctedOmega, Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
const double deltaT) { const Vector3& correctedOmega, const double deltaT) {
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
return deltaRij_new; return deltaRij_new;
} }
@ -94,8 +95,8 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const list<Vector3>& measuredOmegas, const list<double>& deltaTs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
const bool use2ndOrderIntegration = false) { const bool use2ndOrderIntegration = false) {
ImuFactor::PreintegratedMeasurements result(bias, kMeasuredAccCovariance, ImuFactor::PreintegratedMeasurements result(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
use2ndOrderIntegration); use2ndOrderIntegration);
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();
@ -106,29 +107,30 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
return result; return result;
} }
Vector3 evaluatePreintegratedMeasurementsPosition(const imuBias::ConstantBias& bias, Vector3 evaluatePreintegratedMeasurementsPosition(
const list<Vector3>& measuredAccs, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const list<double>& deltaTs) { return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaPij(); deltaTs).deltaPij();
} }
Vector3 evaluatePreintegratedMeasurementsVelocity(const imuBias::ConstantBias& bias, Vector3 evaluatePreintegratedMeasurementsVelocity(
const list<Vector3>& measuredAccs, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const list<double>& deltaTs) { return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaVij(); deltaTs).deltaVij();
} }
Rot3 evaluatePreintegratedMeasurementsRotation(const imuBias::ConstantBias& bias, Rot3 evaluatePreintegratedMeasurementsRotation(
const list<Vector3>& measuredAccs, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const list<double>& deltaTs) {
return Rot3( return Rot3(
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaRij()); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs).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);
} }
@ -136,7 +138,7 @@ 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)));
} }
} // namespace } // namespace
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurements) { TEST(ImuFactor, PreintegratedMeasurements) {
@ -158,19 +160,22 @@ TEST(ImuFactor, PreintegratedMeasurements) {
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, kMeasuredAccCovariance, ImuFactor::PreintegratedMeasurements actual1(bias, kMeasuredAccCovariance,
kMeasuredOmegaCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
kIntegrationErrorCovariance, use2ndOrderIntegration); use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); EXPECT(
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again // Integrate again
Vector3 expectedDeltaP2; Vector3 expectedDeltaP2;
expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5; Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0)
+ expectedDeltaR1.matrix() * measuredAcc * 0.5;
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);
@ -178,42 +183,49 @@ TEST(ImuFactor, PreintegratedMeasurements) {
ImuFactor::PreintegratedMeasurements actual2 = actual1; ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); EXPECT(
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
} }
// Common linearization point and measurements for tests // Common linearization point and measurements for tests
namespace common { namespace common {
imuBias::ConstantBias bias; // Bias imuBias::ConstantBias bias; // Bias
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));
Vector3 v1(Vector3(0.5, 0.0, 0.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.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 / 100.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 measuredOmega(M_PI / 100, 0, 0); Vector3 measuredOmega(M_PI / 100, 0, 0);
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector(); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector();
double deltaT = 1.0; double deltaT = 1.0;
} // namespace common } // namespace common
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobians) { TEST(ImuFactor, ErrorAndJacobians) {
using namespace common; using namespace common;
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pre_int_data( ImuFactor::PreintegratedMeasurements pre_int_data(bias,
bias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, kMeasuredAccCovariance, kMeasuredOmegaCovariance,
use2ndOrderIntegration); kIntegrationErrorCovariance, use2ndOrderIntegration);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, kZeroOmegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
kZeroOmegaCoriolis);
// Expected error // Expected error
Vector errorExpected(9); Vector errorExpected(9);
errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
EXPECT(assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2, bias), 1e-6)); EXPECT(
assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2, bias),
1e-6));
Values values; Values values;
values.insert(X(1), x1); values.insert(X(1), x1);
@ -229,7 +241,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
// Actual Jacobians // Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a; Matrix H1a, H2a, H3a, H4a, H5a;
(void)factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
// Make sure rotation part is correct when error is interpreted as axis-angle // Make sure rotation part is correct when error is interpreted as axis-angle
// Jacobians are around zero, so the rotation part is the same as: // Jacobians are around zero, so the rotation part is the same as:
@ -245,7 +257,9 @@ TEST(ImuFactor, ErrorAndJacobians) {
Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
values.update(V(2), v2_wrong); values.update(V(2), v2_wrong);
errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0;
EXPECT(assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6)); EXPECT(
assert_equal(errorExpected,
factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6));
EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6)); EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6));
// Make sure the whitening is done correctly // Make sure the whitening is done correctly
@ -263,24 +277,28 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
using common::x1; using common::x1;
using common::v1; using common::v1;
using common::v2; using common::v2;
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
// Measurements // Measurements
Vector3 nonZeroOmegaCoriolis; Vector3 nonZeroOmegaCoriolis;
nonZeroOmegaCoriolis << 0, 0.1, 0.1; nonZeroOmegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3; measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3(0.2, 0.0, 0.0); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data( ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredOmegaCovariance, kIntegrationErrorCovariance); kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, nonZeroOmegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
nonZeroOmegaCoriolis);
Values values; Values values;
values.insert(X(1), x1); values.insert(X(1), x1);
@ -299,27 +317,30 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
using common::x1; using common::x1;
using common::v1; using common::v1;
using common::v2; using common::v2;
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
// Measurements // Measurements
Vector3 nonZeroOmegaCoriolis; Vector3 nonZeroOmegaCoriolis;
nonZeroOmegaCoriolis << 0, 0.1, 0.1; nonZeroOmegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3; measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3(0.2, 0.0, 0.0); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data( ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
kMeasuredOmegaCovariance, kIntegrationErrorCovariance); kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
Pose3 bodyPsensor = Pose3(); Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true; bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, nonZeroOmegaCoriolis, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
bodyPsensor, use2ndOrderCoriolis); nonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
Values values; Values values;
values.insert(X(1), x1); values.insert(X(1), x1);
@ -336,7 +357,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PartialDerivative_wrt_Bias) { TEST(ImuFactor, PartialDerivative_wrt_Bias) {
// Linearization point // Linearization point
Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 measuredOmega(0.1, 0, 0); Vector3 measuredOmega(0.1, 0, 0);
@ -344,11 +365,13 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); boost::bind(&evaluateRotation, measuredOmega, _1, deltaT),
Vector3(biasOmega));
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); const Matrix3 Jr = Rot3::ExpmapDerivative(
(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
// 1e-3 needs to be added only when using quaternions for rotations // 1e-3 needs to be added only when using quaternions for rotations
@ -358,7 +381,7 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PartialDerivativeLogmap) { TEST(ImuFactor, PartialDerivativeLogmap) {
// Linearization point // Linearization point
Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 deltatheta(0, 0, 0); Vector3 deltatheta(0, 0, 0);
@ -376,7 +399,7 @@ TEST(ImuFactor, PartialDerivativeLogmap) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, fistOrderExponential) { TEST(ImuFactor, fistOrderExponential) {
// Linearization point // Linearization point
Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 measuredOmega(0.1, 0, 0); Vector3 measuredOmega(0.1, 0, 0);
@ -387,15 +410,18 @@ TEST(ImuFactor, fistOrderExponential) {
Vector3 deltabiasOmega; Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha; deltabiasOmega << alpha, alpha, alpha;
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); const Matrix3 Jr = Rot3::ExpmapDerivative(
(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 = const Matrix expectedRot = Rot3::Expmap(
Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); const Matrix3 hatRot =
const Matrix3 actualRot = hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
// hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); // hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
// This is a first order expansion so the equality is only an approximation // This is a first order expansion so the equality is only an approximation
@ -405,7 +431,7 @@ TEST(ImuFactor, fistOrderExponential) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { TEST(ImuFactor, 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));
@ -420,51 +446,56 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
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
ImuFactor::PreintegratedMeasurements preintegrated = ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs);
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<Vector, imuBias::ConstantBias>( Matrix expectedDelPdelBias = numericalDerivative11<Vector,
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, imuBias::ConstantBias>(
deltaTs), boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
bias); measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<Vector, imuBias::ConstantBias>( Matrix expectedDelVdelBias = numericalDerivative11<Vector,
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, imuBias::ConstantBias>(
deltaTs), boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
bias); measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3, imuBias::ConstantBias>( Matrix expectedDelRdelBias =
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, numericalDerivative11<Rot3, imuBias::ConstantBias>(
deltaTs), boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
bias); measuredAccs, measuredOmegas, deltaTs), 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(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(
assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); EXPECT(
assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), EXPECT(
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// 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 = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements // Measurements
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
@ -477,48 +508,50 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
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);
} }
bool use2ndOrderIntegration = false; bool use2ndOrderIntegration = false;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = evaluatePreintegratedMeasurements( ImuFactor::PreintegratedMeasurements preintegrated =
bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements // so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians // Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01; const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual; Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
Factual, Gactual); newDeltaT, body_P_sensor, Factual, Gactual);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F // COMPUTE NUMERICAL DERIVATIVES FOR F
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions // Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, newMeasuredAcc, boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaPij_old); deltaPij_old);
// Compute expected f_pos_vel wrt velocities // Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, newMeasuredAcc, boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaVij_old); deltaVij_old);
// Compute expected f_pos_vel wrt angles // Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>( Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, newMeasuredAcc, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaRij_old); deltaRij_old);
Matrix FexpectedTop6(6, 9); Matrix FexpectedTop6(6, 9);
@ -526,7 +559,8 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Compute expected f_rot wrt angles // Compute expected f_rot wrt angles
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>( Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), deltaRij_old); boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9); Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
@ -540,25 +574,26 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise // Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6, 3); Matrix dgpv_dintNoise(6, 3);
dgpv_dintNoise << I_3x3* newDeltaT, Z_3x3; dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise // Compute jacobian wrt acc noise
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, _1, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old, _1, newMeasuredOmega, newDeltaT,
newMeasuredAcc); use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise // Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
newMeasuredOmega); newMeasuredOmega);
Matrix GexpectedTop6(6, 9); Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise // Compute expected f_rot wrt gyro noise
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>( Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), newMeasuredOmega); boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9); Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
@ -569,12 +604,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Check covariance propagation // Check covariance propagation
Matrix9 measurementCovariance; Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar* I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar* I_3x3, Z_3x3, measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
Z_3x3, Z_3x3, omegaNoiseVar* I_3x3; * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected = Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
Factual * oldPreintCovariance * Factual.transpose() + * Factual.transpose()
(1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
@ -583,8 +618,8 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// 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 = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements // Measurements
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
@ -597,48 +632,50 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
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);
} }
bool use2ndOrderIntegration = true; bool use2ndOrderIntegration = true;
// Actual preintegrated values // Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated = evaluatePreintegratedMeasurements( ImuFactor::PreintegratedMeasurements preintegrated =
bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements // so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians // Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01; const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual; Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
Factual, Gactual); newDeltaT, body_P_sensor, Factual, Gactual);
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F // COMPUTE NUMERICAL DERIVATIVES FOR F
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions // Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, newMeasuredAcc, boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaPij_old); deltaPij_old);
// Compute expected f_pos_vel wrt velocities // Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>( Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, newMeasuredAcc, boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaVij_old); deltaVij_old);
// Compute expected f_pos_vel wrt angles // Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>( Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, newMeasuredAcc, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaRij_old); deltaRij_old);
Matrix FexpectedTop6(6, 9); Matrix FexpectedTop6(6, 9);
@ -646,7 +683,8 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Compute expected f_rot wrt angles // Compute expected f_rot wrt angles
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>( Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), deltaRij_old); boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9); Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
@ -660,25 +698,26 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise // Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6, 3); Matrix dgpv_dintNoise(6, 3);
dgpv_dintNoise << I_3x3* newDeltaT, Z_3x3; dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise // Compute jacobian wrt acc noise
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, _1, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old, _1, newMeasuredOmega, newDeltaT,
newMeasuredAcc); use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise // Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>( Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
newMeasuredOmega); newMeasuredOmega);
Matrix GexpectedTop6(6, 9); Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise // Compute expected f_rot wrt gyro noise
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>( Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), newMeasuredOmega); boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9); Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
@ -689,12 +728,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Check covariance propagation // Check covariance propagation
Matrix9 measurementCovariance; Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar* I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar* I_3x3, Z_3x3, measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
Z_3x3, Z_3x3, omegaNoiseVar* I_3x3; * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected = Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
Factual * oldPreintCovariance * Factual.transpose() + * Factual.transpose()
(1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
@ -702,10 +741,11 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { TEST(ImuFactor, 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)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements // Measurements
@ -713,19 +753,23 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
nonZeroOmegaCoriolis << 0, 0.1, 0.1; nonZeroOmegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3; measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3(0.2, 0.0, 0.0); Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).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( ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredOmegaCovariance, kIntegrationErrorCovariance); kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, nonZeroOmegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
nonZeroOmegaCoriolis);
Values values; Values values;
values.insert(X(1), x1); values.insert(X(1), x1);
@ -741,11 +785,11 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PredictPositionAndVelocity) { TEST(ImuFactor, PredictPositionAndVelocity) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements // Measurements
Vector3 measuredOmega; Vector3 measuredOmega;
measuredOmega << 0, 0, 0; // M_PI/10.0+0.3; measuredOmega << 0, 0, 0; // M_PI/10.0+0.3;
Vector3 measuredAcc; Vector3 measuredAcc;
measuredAcc << 0, 1, -9.81; measuredAcc << 0, 1, -9.81;
double deltaT = 0.001; double deltaT = 0.001;
@ -754,19 +798,22 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
I6x6 = Matrix::Identity(6, 6); I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data( ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true);
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, kZeroOmegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
kZeroOmegaCoriolis);
// Predict // Predict
Pose3 x1; Pose3 x1;
Vector3 v1(0, 0.0, 0.0); Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, kGravity, kZeroOmegaCoriolis); PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, kGravity,
kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; Vector3 expectedVelocity;
expectedVelocity << 0, 1, 0; expectedVelocity << 0, 1, 0;
@ -776,11 +823,11 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PredictRotation) { TEST(ImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements // Measurements
Vector3 measuredOmega; Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3; measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3;
Vector3 measuredAcc; Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81; measuredAcc << 0, 0, -9.81;
double deltaT = 0.001; double deltaT = 0.001;
@ -789,21 +836,23 @@ TEST(ImuFactor, PredictRotation) {
I6x6 = Matrix::Identity(6, 6); I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data( ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); kMeasuredAccCovariance, kMeasuredOmegaCovariance,
kIntegrationErrorCovariance, true);
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, kZeroOmegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity,
kZeroOmegaCoriolis);
// Predict // Predict
Pose3 x1, x2; Pose3 x1, x2;
Vector3 v1 = Vector3(0, 0.0, 0.0); Vector3 v1 = Vector3(0, 0.0, 0.0);
Vector3 v2; Vector3 v2;
ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), kGravity, ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(),
kZeroOmegaCoriolis); kGravity, kZeroOmegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; Vector3 expectedVelocity;
expectedVelocity << 0, 0, 0; expectedVelocity << 0, 0, 0;