From 9a469ad25f5e487588f8ba72591e014d99bb8899 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sat, 7 Mar 2015 17:47:59 -0500 Subject: [PATCH] Added function predict in ImuFactor and CombinedImuFactor. --- gtsam/navigation/CombinedImuFactor.h | 10 + gtsam/navigation/ImuFactor.h | 9 + gtsam/navigation/PreintegrationBase.h | 32 ++-- .../tests/testCombinedImuFactor.cpp | 4 +- gtsam/navigation/tests/testImuFactor.cpp | 175 +++++++++++++----- 5 files changed, 161 insertions(+), 69 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a7c6ecd39..5f972c318 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -212,6 +212,16 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + static PoseVelocityBias predict(const CombinedPreintegratedMeasurements& PIM, const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + return PreintegrationBase::predict(PIM, pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); + } + + private: /** Serialization function */ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b91c76ade..16582b45d 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -194,6 +194,15 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; + /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + static PoseVelocityBias predict(const PreintegratedMeasurements& PIM, const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) { + return PreintegrationBase::predict(PIM, pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); + } + private: /** Serialization function */ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 29b9b6e66..cf23a3f2a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -199,48 +199,48 @@ public: /// Predict state at time j //------------------------------------------------------------------------------ - PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + static PoseVelocityBias predict(const PreintegrationBase& PIB, const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) const { + boost::optional deltaVij_biascorrected_out = boost::none) { - const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + const Vector3 biasAccIncr = bias_i.accelerometer() - PIB.biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - PIB.biasHat().gyroscope(); const Rot3& Rot_i = pose_i.rotation(); const Vector3& pos_i = pose_i.translation().vector(); // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; + Vector3 deltaPij_biascorrected = PIB.deltaPij() + PIB.delPdelBiasAcc() * biasAccIncr + PIB.delPdelBiasOmega() * biasOmegaIncr; if(deltaPij_biascorrected_out)// if desired, store this *deltaPij_biascorrected_out = deltaPij_biascorrected; Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected - + vel_i * deltaTij() - - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij()*deltaTij(); + + vel_i * PIB.deltaTij() + - omegaCoriolis.cross(vel_i) * PIB.deltaTij()*PIB.deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * PIB.deltaTij()*PIB.deltaTij(); - Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; + Vector3 deltaVij_biascorrected = PIB.deltaVij() + PIB.delVdelBiasAcc() * biasAccIncr + PIB.delVdelBiasOmega() * biasOmegaIncr; if(deltaVij_biascorrected_out)// if desired, store this *deltaVij_biascorrected_out = deltaVij_biascorrected; Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected - - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term - + gravity * deltaTij()); + - 2 * omegaCoriolis.cross(vel_i) * PIB.deltaTij() // Coriolis term + + gravity * PIB.deltaTij()); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity + pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * PIB.deltaTij()*PIB.deltaTij(); // 2nd order coriolis term for position + vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * PIB.deltaTij(); // 2nd order term for velocity } - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + const Rot3 deltaRij_biascorrected = PIB.biascorrectedDeltaRij(biasOmegaIncr); // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); Vector3 correctedOmega = biascorrectedOmega - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + Rot_i.inverse().matrix() * omegaCoriolis * PIB.deltaTij(); // Coriolis term const Rot3 correctedDeltaRij = Rot3::Expmap( correctedOmega ); const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); @@ -273,7 +273,7 @@ public: // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ Vector3 deltaPij_biascorrected, deltaVij_biascorrected; - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + PoseVelocityBias predictedState_j = PreintegrationBase::predict(*this, pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 3f7109543..9fa34ba6e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -294,7 +294,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = PreintegrationBase::predict(Combined_pre_int_data, x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); @@ -322,7 +322,7 @@ TEST(CombinedImuFactor, PredictRotation) { // Predict Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = CombinedImuFactor::predict(Combinedfactor.preintegratedMeasurements(), x,v,bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 9c82a7dfa..3844e923d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -131,8 +131,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation( measuredAccs, measuredOmegas, deltaTs).deltaRij()); } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ - return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); +Rot3 evaluateRotation(const Vector3 b_omegaMeas_nb, const Vector3 biasOmega, const double deltaT){ + return Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT); } Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ @@ -148,8 +148,8 @@ TEST( ImuFactor, PreintegratedMeasurements ) imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases // Measurements - Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 b_accMeas(0.1, 0.0, 0.0); + Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0); double deltaT = 0.5; // Expected preintegrated values @@ -161,7 +161,7 @@ TEST( ImuFactor, PreintegratedMeasurements ) bool use2ndOrderIntegration = true; // Actual preintegrated values ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + actual1.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); @@ -170,13 +170,13 @@ TEST( ImuFactor, PreintegratedMeasurements ) // 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() * b_accMeas * 0.5; Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); double expectedDeltaT2(1); // Actual preintegrated values ImuFactor::PreintegratedMeasurements actual2 = actual1; - actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + actual2.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); @@ -195,17 +195,17 @@ TEST( ImuFactor, ErrorAndJacobians ) Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << M_PI/100, 0, 0; + Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector(); double deltaT = 1.0; bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -262,18 +262,18 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3; + Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).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)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -325,20 +325,20 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3; + Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).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)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // Create factor Pose3 bodyPsensor = Pose3(); bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -387,14 +387,14 @@ TEST( ImuFactor, PartialDerivative_wrt_Bias ) Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0; double deltaT = 0.5; // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); + &evaluateRotation, b_omegaMeas_nb, _1, deltaT), Vector3(biasOmega)); - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((b_omegaMeas_nb - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -428,20 +428,20 @@ TEST( ImuFactor, fistOrderExponential ) Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0; double deltaT = 1.0; // change w.r.t. linearization point double alpha = 0.0; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((b_omegaMeas_nb - biasOmega) * deltaT); 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((b_omegaMeas_nb - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 hatRot = Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT).matrix(); const Matrix3 actualRot = hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); @@ -758,21 +758,21 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) // // // Pre-integrator // imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10)); -// Vector3 gravity; gravity << 0, 0, 9.81; +// Vector3 n_gravity; n_gravity << 0, 0, 9.81; // Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01; // ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); // // // Pre-integrate Measurements -// Vector3 measuredAcc(0.1, 0.0, 0.0); -// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); +// Vector3 b_accMeas(0.1, 0.0, 0.0); +// Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0); // double deltaT = 0.5; // for(size_t i = 0; i < 50; ++i) { -// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // } // // // Create factor // noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.MeasurementCovariance()); -// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); +// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); // // Values values; // values.insert(X(1), x1); @@ -811,10 +811,10 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3; + Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).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)); @@ -822,10 +822,10 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) 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()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); // Expected Jacobians Matrix H1e = numericalDerivative11( @@ -863,10 +863,10 @@ TEST(ImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, 0;//M_PI/10.0+0.3; + Vector3 b_accMeas; b_accMeas << 0,1,-9.81; double deltaT = 0.001; Matrix I6x6(6,6); @@ -875,15 +875,15 @@ TEST(ImuFactor, PredictPositionAndVelocity){ 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(), true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = PreintegrationBase::predict(pre_int_data, x1, v1, bias, n_gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); @@ -895,10 +895,10 @@ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10;//M_PI/10.0+0.3; + Vector3 b_accMeas; b_accMeas << 0,0,-9.81; double deltaT = 0.001; Matrix I6x6(6,6); @@ -907,21 +907,94 @@ TEST(ImuFactor, PredictRotation) { 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(), true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = ImuFactor::predict(factor.preintegratedMeasurements(), x1, v1, bias, n_gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity<<0,0,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } +/* ************************************************************************* */ +#include +#include +#include +#include + +TEST(ImuFactor, CheckBiasCorrection) { + int numFactors = 2; + int numSamplesPreint = 1; + double g = 9.81; + // Measurements. Body frame and nav frame are both z-up + Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0.3, 0.0; + Vector3 b_accMeas; b_accMeas << 0, 0, g; + Vector3 n_gravity; n_gravity << 0, 0, -g; + + // Set up noise and other test params + imuBias::ConstantBias zeroBias(Vector3(0, 0, 0), Vector3(0.0, 0, 0)); // Biases (acc, rot) + Vector6 noiseBetweenBiasSigma; noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = noiseModel::Diagonal::Sigmas(noiseBetweenBiasSigma); + Matrix3 accCov = 1e-4 * I_3x3; + Matrix3 gyroCov = 1e-6 * I_3x3; + Matrix3 integrationCov = 1e-8 * I_3x3; + double deltaT = 0.005; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + + // Specify noise values on priors + Vector6 priorNoisePoseSigmas((Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); + Vector3 priorNoiseVelSigmas((Vector(3) << 0.5, 0.5, 0.5).finished()); + Vector6 priorNoiseBiasSigmas((Vector(6) << 1, 1, 1, 1, 1, 1).finished()); + SharedDiagonal priorNoisePose = noiseModel::Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseVel = noiseModel::Diagonal::Sigmas(priorNoiseVelSigmas); + SharedDiagonal priorNoiseBias = noiseModel::Diagonal::Sigmas(priorNoiseBiasSigmas); + Vector3 zeroVel(0, 0.0, 0.0); + + // Instantiate graph and values + Values values; + NonlinearFactorGraph graph; + + // Add prior factor and values + graph.add(PriorFactor (X(0), Pose3(), priorNoisePose)); + graph.add(PriorFactor(V(0), zeroVel, priorNoiseVel)); + graph.add(PriorFactor(B(0), zeroBias, priorNoiseBias)); + values.insert(X(0), Pose3()); + values.insert(V(0), zeroVel); + values.insert(B(0), zeroBias); + + for (int i = 1; i < numFactors; i++) { + // Preintegrate measurements + ImuFactor::PreintegratedMeasurements pre_int_data = ImuFactor::PreintegratedMeasurements(imuBias::ConstantBias(Vector3(0, 0.0, 0.0), + Vector3(0.0, 0.0, 0.0)), accCov, gyroCov, integrationCov, true); + for (int j = 0; j< numSamplesPreint; ++j) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); + + // Create and add factor + ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, n_gravity, omegaCoriolis); + graph.add(factor); + graph.add(BetweenFactor(B(i-1), B(i), zeroBias, biasNoiseModel)); + + if (i == 30) graph.add(PriorFactor(X(i), Pose3(), priorNoisePose)); + + // Add values + values.insert(X(i), Pose3()); + values.insert(V(i), zeroVel); + values.insert(B(i), zeroBias); + } + // Solve graph and find estimated bias + Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); + imuBias::ConstantBias biasActual = results.at(B(numFactors-1)); +// +// // set expected bias +// imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.3, 0.0)); +// EXPECT(assert_equal(biasExpected, biasActual, 1e-2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */