diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 31ea65ab1..ef94e4141 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -71,7 +71,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, - double deltaT, boost::optional body_P_sensor) { + double deltaT, boost::optional body_P_sensor, + boost::optional F_test, boost::optional G_test) { // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). @@ -129,7 +130,6 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15,15); // BLOCK DIAGONAL TERMS G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); @@ -152,6 +152,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; + + // F_test and G_test are used for testing purposes and are not needed by the factor + if(F_test){ + F_test->resize(15,15); + (*F_test) << F; + } + if(G_test){ + G_test->resize(15,21); + // This is for testing & documentation + ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, + Z_3x3, Z_3x3, H_angles_angles, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3; + } } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3e84c3e67..f7041fdeb 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -126,7 +126,8 @@ public: * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + boost::optional F_test = boost::none, boost::optional G_test = boost::none); /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index b55396041..1842e3d55 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -39,19 +39,44 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; -/* ************************************************************************* */ namespace { +/* ************************************************************************* */ +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedMeasurementsTest( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, + const Vector3& logDeltaRij_old, const imuBias::ConstantBias& bias_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration_) { -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( + Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * correctedAcc * deltaT; + Vector3 deltaPij_new; + if(!use2ndOrderIntegration_){ + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + }else{ + deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); + imuBias::ConstantBias bias_new(bias_old); + Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + return result; +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + const list& deltaTs){ + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -59,7 +84,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } @@ -67,20 +91,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij(); + measuredAccs, measuredOmegas, deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaVij(); } @@ -89,9 +109,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return Rot3(evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaRij()); } @@ -101,7 +119,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation( /* ************************************************************************* */ TEST( CombinedImuFactor, PreintegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases @@ -120,28 +137,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); -// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases -// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) -// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) -// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); -// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol)); -// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol)); -// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol); + EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); + EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } - /* ************************************************************************* */ TEST( CombinedImuFactor, ErrorWithBiases ) { - //cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); @@ -157,50 +163,37 @@ TEST( CombinedImuFactor, ErrorWithBiases ) double deltaT = 1.0; double tol = 1e-6; - // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - // const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - // const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - Matrix I6x6(6,6); I6x6 = Matrix::Identity(6,6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); - Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + // Expected Jacobians + Matrix H1e, H2e, H3e, H4e, H5e; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - - // Expected Jacobians - Matrix H1e, H2e, H3e, H4e, H5e; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - - // Actual Jacobians + // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); @@ -214,7 +207,6 @@ TEST( CombinedImuFactor, ErrorWithBiases ) /* ************************************************************************* */ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases @@ -237,22 +229,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) } // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); // Compute numerical derivatives Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -265,6 +257,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) @@ -283,22 +276,21 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - 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)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); - + // Create factor + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + 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)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) Matrix I6x6(6,6); @@ -324,9 +316,88 @@ TEST(CombinedImuFactor, PredictRotation) { EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } -#include +/* ************************************************************************* */ +TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) +{ + // Linearization point + imuBias::ConstantBias bias = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement +// +// Matrix Factual, Gactual; +// preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, +// body_P_sensor, Factual, Gactual); +// +// bool use2ndOrderIntegration = false; +// +// // Compute expected F wrt positions +// Matrix df_dpos = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// _1, deltaVij_old, logDeltaRij_old, +// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); +// // Compute expected F wrt velocities +// Matrix df_dvel = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, _1, logDeltaRij_old, +// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); +// // Compute expected F wrt angles +// Matrix df_dangle = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, deltaVij_old, _1, +// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); +// Matrix Fexpected(9,9); +// +// Fexpected << df_dpos, df_dvel, df_dangle; +// EXPECT(assert_equal(Fexpected, Factual)); +// +// // Compute expected G wrt integration noise +// Matrix df_dintNoise(9,3); +// df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3; +// +// // Compute expected F wrt acc noise +// Matrix df_daccNoise = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, deltaVij_old, logDeltaRij_old, +// _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); +// // Compute expected F wrt gyro noise +// Matrix df_domegaNoise = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, deltaVij_old, logDeltaRij_old, +// newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); +// Matrix Gexpected(9,9); +// +// Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise; +// EXPECT(assert_equal(Gexpected, Gactual)); +} /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */