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);