From 9a469ad25f5e487588f8ba72591e014d99bb8899 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sat, 7 Mar 2015 17:47:59 -0500 Subject: [PATCH 1/7] 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);} /* ************************************************************************* */ From a814534c9298b960da151dffa7b94aa7b64e74ea Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sun, 8 Mar 2015 15:23:20 -0400 Subject: [PATCH 2/7] Made ImuFactor Predict function static, which calls PreintegrationBase non-static member. Fixed to upper case Predict. --- gtsam/navigation/CombinedImuFactor.h | 6 +- gtsam/navigation/ImuFactor.h | 6 +- gtsam/navigation/PreintegrationBase.h | 32 ++++---- .../tests/testCombinedImuFactor.cpp | 4 +- gtsam/navigation/tests/testImuFactor.cpp | 80 +++++++++---------- 5 files changed, 64 insertions(+), 64 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5f972c318..b752df7f9 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -213,12 +213,12 @@ public: 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, + static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, 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); + return PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 16582b45d..c09e4ee64 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -195,12 +195,12 @@ public: 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, + static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, 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); + return PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); } private: diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index cf23a3f2a..3c30082f6 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -199,48 +199,48 @@ public: /// Predict state at time j //------------------------------------------------------------------------------ - static PoseVelocityBias predict(const PreintegrationBase& PIB, const Pose3& pose_i, const Vector3& vel_i, + PoseVelocityBias Predict(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) { + boost::optional deltaVij_biascorrected_out = boost::none) const { - const Vector3 biasAccIncr = bias_i.accelerometer() - PIB.biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - PIB.biasHat().gyroscope(); + const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - 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 = PIB.deltaPij() + PIB.delPdelBiasAcc() * biasAccIncr + PIB.delPdelBiasOmega() * biasOmegaIncr; + Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + 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 * 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(); + + 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(); - Vector3 deltaVij_biascorrected = PIB.deltaVij() + PIB.delVdelBiasAcc() * biasAccIncr + PIB.delVdelBiasOmega() * biasOmegaIncr; + Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + 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) * PIB.deltaTij() // Coriolis term - + gravity * PIB.deltaTij()); + - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term + + gravity * deltaTij()); if(use2ndOrderCoriolis){ - 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 + 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 } - const Rot3 deltaRij_biascorrected = PIB.biascorrectedDeltaRij(biasOmegaIncr); + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); Vector3 correctedOmega = biascorrectedOmega - - Rot_i.inverse().matrix() * omegaCoriolis * PIB.deltaTij(); // Coriolis term + Rot_i.inverse().matrix() * omegaCoriolis * 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 = PreintegrationBase::predict(*this, pose_i, vel_i, bias_i, gravity, + PoseVelocityBias predictedState_j = Predict(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 9fa34ba6e..809f6f4a9 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 = PreintegrationBase::predict(Combined_pre_int_data, x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.Predict(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 = CombinedImuFactor::predict(Combinedfactor.preintegratedMeasurements(), x,v,bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = CombinedImuFactor::Predict(x,v,bias, Combinedfactor.preintegratedMeasurements(), 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 3844e923d..5c764169b 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 b_omegaMeas_nb, const Vector3 biasOmega, const double deltaT){ - return Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT); +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ + return Rot3::Expmap((measuredOmega - 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 b_accMeas(0.1, 0.0, 0.0); - Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0); + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(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(b_accMeas, b_omegaMeas_nb, deltaT); + 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)); @@ -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() * b_accMeas * 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); // Actual preintegrated values ImuFactor::PreintegratedMeasurements actual2 = actual1; - actual2.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); + 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)); @@ -197,12 +197,12 @@ TEST( ImuFactor, ErrorAndJacobians ) // Measurements Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 b_omegaMeas_nb; b_omegaMeas_nb << M_PI/100, 0, 0; - Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector(); + Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; + Vector3 measuredAcc = 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(b_accMeas, b_omegaMeas_nb, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); @@ -264,13 +264,13 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) // Measurements Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - 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); + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = 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(b_accMeas, b_omegaMeas_nb, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); @@ -327,13 +327,13 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) // Measurements Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - 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); + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = 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(b_accMeas, b_omegaMeas_nb, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor Pose3 bodyPsensor = Pose3(); @@ -387,14 +387,14 @@ TEST( ImuFactor, PartialDerivative_wrt_Bias ) Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias // Measurements - Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0; + Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; double deltaT = 0.5; // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, b_omegaMeas_nb, _1, deltaT), Vector3(biasOmega)); + &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); - const Matrix3 Jr = Rot3::ExpmapDerivative((b_omegaMeas_nb - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - 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 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0; + Vector3 measuredOmega; measuredOmega << 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((b_omegaMeas_nb - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap((b_omegaMeas_nb - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix3 hatRot = Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT).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)); @@ -763,11 +763,11 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) // ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); // // // Pre-integrate Measurements -// Vector3 b_accMeas(0.1, 0.0, 0.0); -// Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0); +// Vector3 measuredAcc(0.1, 0.0, 0.0); +// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); // double deltaT = 0.5; // for(size_t i = 0; i < 50; ++i) { -// pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT); +// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // } // // // Create factor @@ -813,8 +813,8 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) // Measurements Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - 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); + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = 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,7 +822,7 @@ 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(b_accMeas, b_omegaMeas_nb, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); @@ -865,8 +865,8 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Measurements Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - 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; + Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; + Vector3 measuredAcc; measuredAcc << 0,1,-9.81; double deltaT = 0.001; Matrix I6x6(6,6); @@ -875,7 +875,7 @@ 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(b_accMeas, b_omegaMeas_nb, deltaT); + 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, n_gravity, omegaCoriolis); @@ -883,7 +883,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = PreintegrationBase::predict(pre_int_data, x1, v1, bias, n_gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.Predict(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)); @@ -897,8 +897,8 @@ TEST(ImuFactor, PredictRotation) { // Measurements Vector3 n_gravity; n_gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - 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; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3; + Vector3 measuredAcc; measuredAcc << 0,0,-9.81; double deltaT = 0.001; Matrix I6x6(6,6); @@ -907,7 +907,7 @@ 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(b_accMeas, b_omegaMeas_nb, deltaT); + 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, n_gravity, omegaCoriolis); @@ -915,7 +915,7 @@ TEST(ImuFactor, PredictRotation) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = ImuFactor::predict(factor.preintegratedMeasurements(), x1, v1, bias, n_gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = ImuFactor::Predict(x1, v1, bias, factor.preintegratedMeasurements(), 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)); @@ -933,8 +933,8 @@ TEST(ImuFactor, CheckBiasCorrection) { 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 measuredOmega; measuredOmega << 0, 0.3, 0.0; + Vector3 measuredAcc; measuredAcc << 0, 0, g; Vector3 n_gravity; n_gravity << 0, 0, -g; // Set up noise and other test params @@ -972,7 +972,7 @@ TEST(ImuFactor, CheckBiasCorrection) { // 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); + for (int j = 0; j< numSamplesPreint; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, 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); From 6861b8dd01350b0bd424110f8c7b09765affb504 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sun, 8 Mar 2015 15:31:02 -0400 Subject: [PATCH 3/7] reverted to previous gravity name --- gtsam/navigation/tests/testImuFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 5c764169b..98578165b 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -935,7 +935,7 @@ TEST(ImuFactor, CheckBiasCorrection) { // Measurements. Body frame and nav frame are both z-up Vector3 measuredOmega; measuredOmega << 0, 0.3, 0.0; Vector3 measuredAcc; measuredAcc << 0, 0, g; - Vector3 n_gravity; n_gravity << 0, 0, -g; + Vector3 gravity; 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) @@ -975,7 +975,7 @@ TEST(ImuFactor, CheckBiasCorrection) { for (int j = 0; j< numSamplesPreint; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, 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); + ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, gravity, omegaCoriolis); graph.add(factor); graph.add(BetweenFactor(B(i-1), B(i), zeroBias, biasNoiseModel)); From 5d739ea904f8e9a7f43a0c1a3e546b5437d0aa0a Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sun, 8 Mar 2015 15:33:43 -0400 Subject: [PATCH 4/7] Earlier refactor did not work correctly --- gtsam/navigation/tests/testImuFactor.cpp | 40 ++++++++++++------------ 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 98578165b..4b55e0ee3 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -195,17 +195,17 @@ TEST( ImuFactor, ErrorAndJacobians ) Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 n_gravity; n_gravity << 0, 0, 9.81; + Vector3 gravity; 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(n_gravity)).vector(); + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(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); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -262,10 +262,10 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 n_gravity; n_gravity << 0, 0, 9.81; + Vector3 gravity; 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(n_gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(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), @@ -273,7 +273,7 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -325,10 +325,10 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 n_gravity; n_gravity << 0, 0, 9.81; + Vector3 gravity; 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(n_gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(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), @@ -338,7 +338,7 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) // Create factor Pose3 bodyPsensor = Pose3(); bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -758,7 +758,7 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) // // // Pre-integrator // imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10)); -// Vector3 n_gravity; n_gravity << 0, 0, 9.81; +// Vector3 gravity; 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()); // @@ -772,7 +772,7 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) // // // 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, n_gravity, omegaCoriolis); +// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, 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 n_gravity; n_gravity << 0, 0, 9.81; + Vector3 gravity; 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(n_gravity)).vector() + Vector3(0.2,0.0,0.0); + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(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)); @@ -825,7 +825,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Expected Jacobians Matrix H1e = numericalDerivative11( @@ -863,7 +863,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 n_gravity; n_gravity << 0, 0, 9.81; + Vector3 gravity; 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; @@ -878,12 +878,12 @@ TEST(ImuFactor, PredictPositionAndVelocity){ 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, n_gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.Predict(x1, v1, bias, n_gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.Predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); @@ -895,7 +895,7 @@ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 n_gravity; n_gravity << 0, 0, 9.81; + Vector3 gravity; 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; @@ -910,12 +910,12 @@ TEST(ImuFactor, PredictRotation) { 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, n_gravity, omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = ImuFactor::Predict(x1, v1, bias, factor.preintegratedMeasurements(), n_gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = ImuFactor::Predict(x1, v1, bias, factor.preintegratedMeasurements(), 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)); From 736fce27db5cef032ca8d2775357999f5e9b4abf Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sun, 8 Mar 2015 15:35:01 -0400 Subject: [PATCH 5/7] Test not needed for the purposes of the P.R --- gtsam/navigation/tests/testImuFactor.cpp | 73 ------------------------ 1 file changed, 73 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 4b55e0ee3..0b672608d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -922,79 +922,6 @@ TEST(ImuFactor, PredictRotation) { 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 measuredOmega; measuredOmega << 0, 0.3, 0.0; - Vector3 measuredAcc; measuredAcc << 0, 0, g; - Vector3 gravity; 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(measuredAcc, measuredOmega, deltaT); - - // Create and add factor - ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, 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);} /* ************************************************************************* */ From ab21ee9507f77e91ce8f8c12f82d5f244917d13c Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sun, 8 Mar 2015 22:07:14 -0400 Subject: [PATCH 6/7] Made api backwards compatible. --- gtsam/navigation/CombinedImuFactor.h | 6 ++++-- gtsam/navigation/ImuFactor.h | 8 +++++--- gtsam/navigation/tests/testCombinedImuFactor.cpp | 8 ++++---- gtsam/navigation/tests/testImuFactor.cpp | 11 ++++++----- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index b752df7f9..6eb4de58b 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -213,12 +213,14 @@ public: 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 Pose3& pose_i, const Vector3& vel_i, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, 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 PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); + PoseVelocityBias PVB(PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c09e4ee64..4369a411f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -195,12 +195,14 @@ public: 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 Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, 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 PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out); + PoseVelocityBias PVB(PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; } private: diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 809f6f4a9..70686acfd 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -320,11 +320,11 @@ TEST(CombinedImuFactor, PredictRotation) { Combined_pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); - Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = CombinedImuFactor::Predict(x,v,bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); + Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)), x2; + Vector3 v(0,0,0), v2; + CombinedImuFactor::Predict(x, v, x2, v2, bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); + EXPECT(assert_equal(expectedPose, x2, tol)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 0b672608d..b1dbd504f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -913,13 +913,14 @@ TEST(ImuFactor, PredictRotation) { ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = ImuFactor::Predict(x1, v1, bias, factor.preintegratedMeasurements(), gravity, omegaCoriolis); + Pose3 x1, x2; + Vector3 v1 = Vector3(0, 0.0, 0.0); + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), 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))); + EXPECT(assert_equal(expectedPose, x2)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); } /* ************************************************************************* */ From 4f1eb63b0212c17339e290c619440b7ae2702928 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sun, 8 Mar 2015 23:52:47 -0400 Subject: [PATCH 7/7] Fixed naming in non-static data member. --- gtsam/navigation/CombinedImuFactor.h | 2 +- gtsam/navigation/ImuFactor.h | 2 +- gtsam/navigation/PreintegrationBase.h | 4 ++-- gtsam/navigation/tests/testCombinedImuFactor.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6eb4de58b..d2ad32122 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -218,7 +218,7 @@ public: const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB(PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); + PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); pose_j = PVB.pose; vel_j = PVB.velocity; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 4369a411f..984d62d08 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -200,7 +200,7 @@ public: const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB(PIM.Predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); + PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); pose_j = PVB.pose; vel_j = PVB.velocity; } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3c30082f6..29b9b6e66 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -199,7 +199,7 @@ public: /// Predict state at time j //------------------------------------------------------------------------------ - PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + PoseVelocityBias predict(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, @@ -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 = predict(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 70686acfd..c6eb1396e 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 = Combined_pre_int_data.predict(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)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index b1dbd504f..62f496894 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -883,7 +883,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.Predict(x1, v1, bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose));