diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index dc9ae288a..f7acfa79e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -18,16 +18,16 @@ * @author Stephen Williams */ -#include -#include -#include -#include -#include -#include +#include #include #include - -#include +#include +#include +#include +#include +#include +#include +#include #include @@ -44,207 +44,272 @@ static boost::shared_ptr Params() { } } -/* ************************************************************************* */ -TEST( CombinedImuFactor, PreintegratedMeasurements ) { - // Linearization point - Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases +// /* ************************************************************************* */ +// TEST(CombinedImuFactor, PreintegratedMeasurements ) { +// // Linearization point +// Bias 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); - double deltaT = 0.5; - double tol = 1e-6; +// // Measurements +// Vector3 measuredAcc(0.1, 0.0, 0.0); +// Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); +// double deltaT = 0.5; +// double tol = 1e-6; - auto p = testing::Params(); +// auto p = testing::Params(); - // Actual preintegrated values - PreintegratedImuMeasurements expected1(p, bias); - expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// // Actual preintegrated values +// PreintegratedImuMeasurements expected1(p, bias); +// expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - PreintegratedCombinedMeasurements actual1(p, bias); +// PreintegratedCombinedMeasurements actual1(p, bias); - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol)); - EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol)); - EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol)); - DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); -} +// EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol)); +// EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol)); +// EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol)); +// DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); +// } -/* ************************************************************************* */ -TEST( CombinedImuFactor, ErrorWithBiases ) { - Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Bias 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)); - Vector3 v1(0.5, 0.0, 0.0); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), - Point3(5.5, 1.0, -50.0)); - Vector3 v2(0.5, 0.0, 0.0); +// /* ************************************************************************* */ +// TEST(CombinedImuFactor, Accelerating) { +// const double a = 0.2, v = 50; - auto p = testing::Params(); - p->omegaCoriolis = Vector3(0,0.1,0.1); - PreintegratedImuMeasurements pim( - p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); +// // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X +// // The body itself has Z axis pointing down +// const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +// const Point3 initial_position(10, 20, 0); +// const Vector3 initial_velocity(v, 0, 0); - // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = - x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0); - double deltaT = 1.0; - double tol = 1e-6; +// const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, +// Vector3(a, 0, 0)); - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// const double T = 3.0; // seconds +// CombinedScenarioRunner runner(scenario, testing::Params(), T / 10); - PreintegratedCombinedMeasurements combined_pim(p, - Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); +// PreintegratedCombinedMeasurements pim = runner.integrate(T); +// EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); - combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// auto estimatedCov = runner.estimateCovariance(T, 100); +// Eigen::Matrix expected = pim.preintMeasCov(); +// // EXPECT(assert_equal(estimatedCov, expected, 0.1)); +// } - // Create factor - ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim); +// /* ************************************************************************* */ +// TEST(CombinedImuFactor, ErrorWithBiases ) { +// Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) +// Bias 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)); +// Vector3 v1(0.5, 0.0, 0.0); +// Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), +// Point3(5.5, 1.0, -50.0)); +// Vector3 v2(0.5, 0.0, 0.0); - noiseModel::Gaussian::shared_ptr Combinedmodel = - noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov()); - CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), - combined_pim); +// auto p = testing::Params(); +// p->omegaCoriolis = Vector3(0,0.1,0.1); +// PreintegratedImuMeasurements pim( +// p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); - Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); - Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, - bias2); - EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); +// // Measurements +// Vector3 measuredOmega; +// measuredOmega << 0, 0, M_PI / 10.0 + 0.3; +// Vector3 measuredAcc = +// x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0); +// double deltaT = 1.0; +// double tol = 1e-6; - // Expected Jacobians - Matrix H1e, H2e, H3e, H4e, H5e; - (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); +// pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, - H3a, H4a, H5a, H6a); +// PreintegratedCombinedMeasurements combined_pim(p, +// Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); - EXPECT(assert_equal(H1e, H1a.topRows(9))); - EXPECT(assert_equal(H2e, H2a.topRows(9))); - EXPECT(assert_equal(H3e, H3a.topRows(9))); - EXPECT(assert_equal(H4e, H4a.topRows(9))); - EXPECT(assert_equal(H5e, H5a.topRows(9))); -} +// combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); -/* ************************************************************************* */ -#ifdef GTSAM_TANGENT_PREINTEGRATION -TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { - auto p = testing::Params(); - testing::SomeMeasurements measurements; +// // Create factor +// ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim); - auto preintegrated = [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(p, Bias(a, w)); - testing::integrateMeasurements(measurements, &pim); - return pim.preintegrated(); - }; +// noiseModel::Gaussian::shared_ptr Combinedmodel = +// noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov()); +// CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), +// combined_pim); - // Actual pre-integrated values - PreintegratedCombinedMeasurements pim(p); - testing::integrateMeasurements(measurements, &pim); +// Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); +// Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, +// bias2); +// EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), - pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), - pim.preintegrated_H_biasOmega(), 1e-3)); -} -#endif +// // Expected Jacobians +// Matrix H1e, H2e, H3e, H4e, H5e; +// (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); -/* ************************************************************************* */ -TEST(CombinedImuFactor, PredictPositionAndVelocity) { - const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) +// // Actual Jacobians +// Matrix H1a, H2a, H3a, H4a, H5a, H6a; +// (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, +// H3a, H4a, H5a, H6a); - auto p = testing::Params(); +// EXPECT(assert_equal(H1e, H1a.topRows(9))); +// EXPECT(assert_equal(H2e, H2a.topRows(9))); +// EXPECT(assert_equal(H3e, H3a.topRows(9))); +// EXPECT(assert_equal(H4e, H4a.topRows(9))); +// EXPECT(assert_equal(H5e, H5a.topRows(9))); +// } - // Measurements - const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3; - const Vector3 measuredAcc(0, 1.1, -kGravity); - const double deltaT = 0.01; +// /* ************************************************************************* */ +// #ifdef GTSAM_TANGENT_PREINTEGRATION +// TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { +// auto p = testing::Params(); +// testing::SomeMeasurements measurements; - PreintegratedCombinedMeasurements pim(p, bias); +// auto preintegrated = [=](const Vector3& a, const Vector3& w) { +// PreintegratedImuMeasurements pim(p, Bias(a, w)); +// testing::integrateMeasurements(measurements, &pim); +// return pim.preintegrated(); +// }; - for (int i = 0; i < 100; ++i) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// // Actual pre-integrated values +// PreintegratedCombinedMeasurements pim(p); +// testing::integrateMeasurements(measurements, &pim); - // Create factor - const noiseModel::Gaussian::shared_ptr combinedmodel = - noiseModel::Gaussian::Covariance(pim.preintMeasCov()); - const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); +// EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), +// pim.preintegrated_H_biasAcc())); +// EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), +// pim.preintegrated_H_biasOmega(), 1e-3)); +// } +// #endif - // Predict - const NavState actual = pim.predict(NavState(), bias); - const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - const Vector3 expectedVelocity(0, 1, 0); - EXPECT(assert_equal(expectedPose, actual.pose())); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); -} +// /* ************************************************************************* */ +// TEST(CombinedImuFactor, PredictPositionAndVelocity) { +// const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) -/* ************************************************************************* */ -TEST(CombinedImuFactor, PredictRotation) { - const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - auto p = testing::Params(); - PreintegratedCombinedMeasurements pim(p, bias); - const Vector3 measuredAcc = - kGravityAlongNavZDown; - const Vector3 measuredOmega(0, 0, M_PI / 10.0); - const double deltaT = 0.01; - const double tol = 1e-4; - for (int i = 0; i < 100; ++i) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); +// auto p = testing::Params(); - // Predict - const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; - const Vector3 v(0, 0, 0), v2; - const NavState actual = pim.predict(NavState(x, v), bias); - const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); - EXPECT(assert_equal(expectedPose, actual.pose(), tol)); -} +// // Measurements +// const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3; +// const Vector3 measuredAcc(0, 1.1, -kGravity); +// const double deltaT = 0.01; -/* ************************************************************************* */ -// Testing covariance to check if all the jacobians are accounted for. -TEST(CombinedImuFactor, CheckCovariance) { - auto params = PreintegrationCombinedParams::MakeSharedU(9.81); +// PreintegratedCombinedMeasurements pim(p, bias); + +// for (int i = 0; i < 100; ++i) +// pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + +// // Create factor +// const noiseModel::Gaussian::shared_ptr combinedmodel = +// noiseModel::Gaussian::Covariance(pim.preintMeasCov()); +// const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); + +// // Predict +// const NavState actual = pim.predict(NavState(), bias); +// const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); +// const Vector3 expectedVelocity(0, 1, 0); +// EXPECT(assert_equal(expectedPose, actual.pose())); +// EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity()))); +// } + +// /* ************************************************************************* */ +// TEST(CombinedImuFactor, PredictRotation) { +// const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) +// auto p = testing::Params(); +// PreintegratedCombinedMeasurements pim(p, bias); +// const Vector3 measuredAcc = - kGravityAlongNavZDown; +// const Vector3 measuredOmega(0, 0, M_PI / 10.0); +// const double deltaT = 0.01; +// const double tol = 1e-4; +// for (int i = 0; i < 100; ++i) +// pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim); + +// // Predict +// const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; +// const Vector3 v(0, 0, 0), v2; +// const NavState actual = pim.predict(NavState(x, v), bias); +// const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); +// EXPECT(assert_equal(expectedPose, actual.pose(), tol)); +// } + +// /* ************************************************************************* */ +// // Testing covariance to check if all the jacobians are accounted for. +// TEST(CombinedImuFactor, CheckCovariance) { +// auto params = PreintegrationCombinedParams::MakeSharedU(9.81); + +// params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); +// params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3); +// params->setIntegrationCovariance(pow(0.0, 2) * I_3x3); +// params->setOmegaCoriolis(Vector3::Zero()); + +// imuBias::ConstantBias currentBias; + +// PreintegratedCombinedMeasurements actual(params, currentBias); + +// // Measurements +// Vector3 measuredAcc(0.1577, -0.8251, 9.6111); +// Vector3 measuredOmega(-0.0210, 0.0311, 0.0145); +// double deltaT = 0.01; + +// actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + +// Eigen::Matrix expected; +// expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, // +// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01; + +// // regression +// EXPECT(assert_equal(expected, actual.preintMeasCov())); +// } + +TEST(CombinedImuFactor, SameCovarianceCombined) { + auto params = PreintegrationCombinedParams::MakeSharedU(); params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3); - params->setIntegrationCovariance(pow(0.0, 2) * I_3x3); + params->setIntegrationCovariance(pow(0, 2) * I_3x3); params->setOmegaCoriolis(Vector3::Zero()); imuBias::ConstantBias currentBias; - PreintegratedCombinedMeasurements actual(params, currentBias); + PreintegratedCombinedMeasurements pim(params, currentBias); - // Measurements - Vector3 measuredAcc(0.1577, -0.8251, 9.6111); - Vector3 measuredOmega(-0.0210, 0.0311, 0.0145); + Vector3 accMeas(0.1577, -0.8251, 9.6111); + Vector3 omegaMeas(-0.0210, 0.0311, 0.0145); double deltaT = 0.01; + pim.integrateMeasurement(accMeas, omegaMeas, deltaT); - actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - Eigen::Matrix expected; - expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01; - - // regression - EXPECT(assert_equal(expected, actual.preintMeasCov())); + std::cout << pim.preintMeasCov() << std::endl << std::endl; } +TEST(CombinedImuFactor, SameCovariance) { + auto params = PreintegrationParams::MakeSharedU(); + + params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); + params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3); + params->setIntegrationCovariance(pow(0, 2) * I_3x3); + params->setOmegaCoriolis(Vector3::Zero()); + + imuBias::ConstantBias currentBias; + + PreintegratedImuMeasurements pim(params, currentBias); + + Vector3 accMeas(0.1577, -0.8251, 9.6111); + Vector3 omegaMeas(-0.0210, 0.0311, 0.0145); + double deltaT = 0.01; + pim.integrateMeasurement(accMeas, omegaMeas, deltaT); + + std::cout << pim.preintMeasCov() << std::endl << std::endl; +} + + /* ************************************************************************* */ int main() { TestResult tr;