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