From 77f69146f65e0ff179e538f0877f1cb62abbe24f Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 2 Mar 2015 15:14:05 -0500 Subject: [PATCH 1/6] Unit test that fails! Bias estimated is opposite in direction for test bodyPSensorWithBias! Not equal: expected: .biasAcc [0 0 0] expected: .biasGyro [ 0 0.01 0] actual: .biasAcc [-0.00012 3.6e-16 0.00015] actual: .biasGyro [-3.7e-18 -0.01 -3.6e-18] --- gtsam/navigation/tests/testImuFactor.cpp | 135 +++++++++++++++++++++++ 1 file changed, 135 insertions(+) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 9c82a7dfa..ecf019a04 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -922,6 +922,141 @@ TEST(ImuFactor, PredictRotation) { EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } +/* ************************************************************************* */ +TEST(ImuFactor, bodyPSensorNoBias) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + // Measurements + Vector3 n_gravity; n_gravity << 0, 0, -9.81; // z-up nav frame + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 s_omegaMeas_ns; s_omegaMeas_ns << 0, 0.1, M_PI/10; + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force + // w.r.t gravity + Vector3 s_accMeas; s_accMeas << 0,0,-9.81; + double dt = 0.001; + Pose3 b_P_s(Rot3::ypr(0,0,M_PI), Point3(0,0,0)); // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + + Matrix I6x6(6,6); + I6x6 = Matrix::Identity(6,6); + + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), true); + + for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, b_P_s); + + // Create factor + 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, 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 +#include + +TEST(ImuFactor, bodyPSensorWithBias) { + int numFactors = 80; + 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); + + // Measurements + Vector3 n_gravity; n_gravity << 0, 0, -9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega; measuredOmega << 0, 0.01, 0.0; + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force + // w.r.t gravity + Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + + Pose3 bPs(Rot3::ypr(0,0,M_PI), Point3()); + + Matrix3 accCov = 1e-4 * I_3x3; + Matrix3 gyroCov = 1e-6 * I_3x3; + Matrix3 integrationCov = 1e-8 * I_3x3; + double deltaT = 0.005; + + // 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.1, 0.1, 0.1).finished()); + Vector6 priorNoiseBiasSigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-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); + + + + Values values; + NonlinearFactorGraph graph; + + PriorFactor priorPose(X(0), Pose3(), priorNoisePose); + graph.add(priorPose); + values.insert(X(0), Pose3()); + + PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); + graph.add(priorVel); + values.insert(V(0), zeroVel); + + PriorFactor priorBias(B(0), zeroBias, priorNoiseBias); + graph.add(priorBias); + values.insert(B(0), zeroBias); + + for (int i = 1; i < numFactors; i++) { + 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<200; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT, bPs); + + // Create 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)); + + values.insert(X(i), Pose3()); + values.insert(V(i), zeroVel); + values.insert(B(i), zeroBias); + } + Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); + cout.precision(2); + Marginals marginals(graph, results); + imuBias::ConstantBias biasActual = results.at(B(numFactors-1)); + + imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.01, 0.0)); + EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); + +// results.print("Results: \n"); +// +// for (int i = 0; i < numFactors; i++) { +// imuBias::ConstantBias currentBias = results.at(B(i)); +// cout << "currentBiasEstimate: " << currentBias.vector().transpose() << endl; +// } +// for (int i = 0; i < numFactors; i++) { +// Matrix biasCov = marginals.marginalCovariance(B(i)); +// cout << "b" << i << " cov: " << biasCov.diagonal().transpose() << endl; +// } +// +// for (int i = 0; i < numFactors; i++) { +// Pose3 currentPose = results.at(X(i)); +// cout << "currentYPREstimate: " << currentPose.rotation().ypr().transpose()*180/M_PI << endl; +// } +// for (int i = 0; i < numFactors; i++) +// cout << "x" << i << " covariance: " << marginals.marginalCovariance(X(i)).diagonal().transpose() << endl; +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 04cf6686b4c1205ce322d22e2d2bda8d3cead666 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 2 Mar 2015 16:55:58 -0500 Subject: [PATCH 2/6] Better noise values, little cleanup --- gtsam/navigation/tests/testImuFactor.cpp | 43 ++++++++++++------------ 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index ecf019a04..b47612088 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -968,7 +968,6 @@ TEST(ImuFactor, bodyPSensorNoBias) { TEST(ImuFactor, bodyPSensorWithBias) { int numFactors = 80; - 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); @@ -985,9 +984,9 @@ TEST(ImuFactor, bodyPSensorWithBias) { Pose3 bPs(Rot3::ypr(0,0,M_PI), Point3()); - Matrix3 accCov = 1e-4 * I_3x3; - Matrix3 gyroCov = 1e-6 * I_3x3; - Matrix3 integrationCov = 1e-8 * I_3x3; + Matrix3 accCov = 1e-7 * I_3x3; + Matrix3 gyroCov = 1e-8 * I_3x3; + Matrix3 integrationCov = 1e-9 * I_3x3; double deltaT = 0.005; // Specify noise values on priors @@ -1012,9 +1011,10 @@ TEST(ImuFactor, bodyPSensorWithBias) { graph.add(priorVel); values.insert(V(0), zeroVel); - PriorFactor priorBias(B(0), zeroBias, priorNoiseBias); - graph.add(priorBias); - values.insert(B(0), zeroBias); + imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0.0, 0.01, 0)); // Biases (acc, rot) + PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); + graph.add(priorBiasFactor); + values.insert(B(0), priorBias); for (int i = 1; i < numFactors; i++) { ImuFactor::PreintegratedMeasurements pre_int_data = ImuFactor::PreintegratedMeasurements(imuBias::ConstantBias(Vector3(0, 0.0, 0.0), @@ -1024,37 +1024,38 @@ TEST(ImuFactor, bodyPSensorWithBias) { // Create 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)); + imuBias::ConstantBias betweenBias(Vector3(0, 0, 0), Vector3(0.0, 0, 0)); + graph.add(BetweenFactor(B(i-1), B(i), betweenBias, biasNoiseModel)); values.insert(X(i), Pose3()); values.insert(V(i), zeroVel); - values.insert(B(i), zeroBias); + values.insert(B(i), priorBias); } Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); cout.precision(2); Marginals marginals(graph, results); imuBias::ConstantBias biasActual = results.at(B(numFactors-1)); - imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.01, 0.0)); - EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); + results.print("Results: \n"); -// results.print("Results: \n"); -// -// for (int i = 0; i < numFactors; i++) { -// imuBias::ConstantBias currentBias = results.at(B(i)); -// cout << "currentBiasEstimate: " << currentBias.vector().transpose() << endl; -// } + for (int i = 0; i < numFactors; i++) { + imuBias::ConstantBias currentBias = results.at(B(i)); + cout << "currentBiasEstimate: " << currentBias.vector().transpose() << endl; + } // for (int i = 0; i < numFactors; i++) { // Matrix biasCov = marginals.marginalCovariance(B(i)); // cout << "b" << i << " cov: " << biasCov.diagonal().transpose() << endl; // } // -// for (int i = 0; i < numFactors; i++) { -// Pose3 currentPose = results.at(X(i)); -// cout << "currentYPREstimate: " << currentPose.rotation().ypr().transpose()*180/M_PI << endl; -// } + for (int i = 0; i < numFactors; i++) { + Pose3 currentPose = results.at(X(i)); + cout << "currentYPREstimate (in Deg): " << currentPose.rotation().ypr().transpose()*180/M_PI << endl; + } // for (int i = 0; i < numFactors; i++) // cout << "x" << i << " covariance: " << marginals.marginalCovariance(X(i)).diagonal().transpose() << endl; + + imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.01, 0.0)); + EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); } /* ************************************************************************* */ From 0fafffb02e9ac536244f1db2e1aa1b334a7e937d Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 2 Mar 2015 17:04:19 -0500 Subject: [PATCH 3/6] Made functional correctMeasurementsByBiasAndSensorPose --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/PreintegrationBase.h | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1c22bab9a..edb222261 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -78,7 +78,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 81120b7c6..4adf324c7 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -67,7 +67,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 29b9b6e66..08c16846a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -180,9 +180,10 @@ public: update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); } - void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { + boost::tuple + correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega, boost::optional body_P_sensor) { + Vector3 correctedAcc, correctedOmega; correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedOmega = biasHat_.correctGyroscope(measuredOmega); @@ -195,6 +196,7 @@ public: correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } + return boost::make_tuple(correctedAcc, correctedOmega); } /// Predict state at time j From 0310eb4e7b63cbb131f6d5fc624d2e706218fd79 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 25 Jul 2015 10:02:38 +0200 Subject: [PATCH 4/6] Fixed compilation errors --- gtsam/navigation/PreintegrationBase.cpp | 13 ++++++++----- gtsam/navigation/PreintegrationBase.h | 20 +++----------------- 2 files changed, 11 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 496569599..108063c22 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -126,9 +126,10 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); } -void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { +std::pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( + const Vector3& measuredAcc, const Vector3& measuredOmega, + boost::optional body_P_sensor) const { + Vector3 correctedAcc, correctedOmega; correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedOmega = biasHat_.correctGyroscope(measuredOmega); @@ -136,12 +137,14 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( // (originally in the IMU frame) into the body frame if (body_P_sensor) { Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); correctedAcc = body_R_sensor * correctedAcc - - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + - body_omega_body__cross * body_omega_body__cross + * body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } + return std::make_pair(correctedAcc, correctedOmega); } /// Predict state at time j diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 1b2d6f783..3528231bf 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -133,24 +133,10 @@ class PreintegrationBase : public PreintegratedRotation { const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT); - boost::tuple + std::pair correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, boost::optional body_P_sensor) { - Vector3 correctedAcc, correctedOmega; - correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - return boost::make_tuple(correctedAcc, correctedOmega); - } + const Vector3& measuredOmega, + boost::optional body_P_sensor) const; /// Predict state at time j PoseVelocityBias predict( From 18ff25b67f184786660774d8422e79c6e26f1e9c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 25 Jul 2015 17:34:16 +0200 Subject: [PATCH 5/6] Cleaned up test while reading it --- gtsam/navigation/tests/testImuFactor.cpp | 145 +++++++++++------------ 1 file changed, 72 insertions(+), 73 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 964693547..db54fc1fd 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -1051,36 +1051,38 @@ TEST(ImuFactor, bodyPSensorNoBias) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Measurements - Vector3 n_gravity; n_gravity << 0, 0, -9.81; // z-up nav frame - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + Vector3 n_gravity(0, 0, -9.81); // z-up nav frame + Vector3 omegaCoriolis(0, 0, 0); // Sensor frame is z-down // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame - Vector3 s_omegaMeas_ns; s_omegaMeas_ns << 0, 0.1, M_PI/10; - // Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force - // w.r.t gravity - Vector3 s_accMeas; s_accMeas << 0,0,-9.81; + Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 s_accMeas(0, 0, -9.81); double dt = 0.001; - Pose3 b_P_s(Rot3::ypr(0,0,M_PI), Point3(0,0,0)); // Rotate sensor (z-down) to body (same as navigation) i.e. z-up - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), true); + ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, b_P_s); + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(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))); + // Predict + Pose3 x1; + Vector3 v1(0, 0, 0); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, + omegaCoriolis); + + Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + + Vector3 expectedVelocity(0, 0, 0); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); } /* ************************************************************************* */ @@ -1091,22 +1093,27 @@ TEST(ImuFactor, bodyPSensorNoBias) { #include TEST(ImuFactor, bodyPSensorWithBias) { + using noiseModel::Diagonal; + typedef imuBias::ConstantBias Bias; + int numFactors = 80; - 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); + 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 = Diagonal::Sigmas(noiseBetweenBiasSigma); // Measurements - Vector3 n_gravity; n_gravity << 0, 0, -9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + Vector3 n_gravity(0, 0, -9.81); + Vector3 omegaCoriolis(0, 0, 0); // Sensor frame is z-down // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame - Vector3 measuredOmega; measuredOmega << 0, 0.01, 0.0; - // Acc measurement is acceleration of sensor in the sensor frame, when stationary, table exerts an equal and opposite force - // w.r.t gravity - Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + Vector3 measuredOmega(0, 0.01, 0); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 measuredAcc(0, 0, -9.81); - Pose3 bPs(Rot3::ypr(0,0,M_PI), Point3()); + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); Matrix3 accCov = 1e-7 * I_3x3; Matrix3 gyroCov = 1e-8 * I_3x3; @@ -1114,18 +1121,19 @@ TEST(ImuFactor, bodyPSensorWithBias) { double deltaT = 0.005; // 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.1, 0.1, 0.1).finished()); - Vector6 priorNoiseBiasSigmas((Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-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); + Vector6 priorNoisePoseSigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); + Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); + Vector6 priorNoiseBiasSigmas( + (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished()); + SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas); + SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); + Vector3 zeroVel(0, 0, 0); - - - Values values; + // Create a factor graph with priors on initial pose, vlocity and bias NonlinearFactorGraph graph; + Values values; PriorFactor priorPose(X(0), Pose3(), priorNoisePose); graph.add(priorPose); @@ -1135,50 +1143,41 @@ TEST(ImuFactor, bodyPSensorWithBias) { graph.add(priorVel); values.insert(V(0), zeroVel); - imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0.0, 0.01, 0)); // Biases (acc, rot) - PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); + // The key to this test is that we specify the bias, in the sensor frame, as known a priori + // We also create factors below that encode our assumption that this bias is constant over time + // In theory, after optimization, we should recover that same bias estimate + Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); graph.add(priorBiasFactor); values.insert(B(0), priorBias); + // Now add IMU factors and bias noise models + Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); for (int i = 1; i < numFactors; i++) { - 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<200; ++j) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT, bPs); + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(zeroBias, accCov, gyroCov, + integrationCov, true); + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); - // Create 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); - imuBias::ConstantBias betweenBias(Vector3(0, 0, 0), Vector3(0.0, 0, 0)); - graph.add(BetweenFactor(B(i-1), B(i), betweenBias, biasNoiseModel)); + // Create factors + graph.add( + ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, + omegaCoriolis)); + graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); values.insert(X(i), Pose3()); values.insert(V(i), zeroVel); values.insert(B(i), priorBias); } + + // Finally, optimize, and get bias at last time step Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); - cout.precision(2); - Marginals marginals(graph, results); - imuBias::ConstantBias biasActual = results.at(B(numFactors-1)); + Bias biasActual = results.at(B(numFactors - 1)); - results.print("Results: \n"); - - for (int i = 0; i < numFactors; i++) { - imuBias::ConstantBias currentBias = results.at(B(i)); - cout << "currentBiasEstimate: " << currentBias.vector().transpose() << endl; - } -// for (int i = 0; i < numFactors; i++) { -// Matrix biasCov = marginals.marginalCovariance(B(i)); -// cout << "b" << i << " cov: " << biasCov.diagonal().transpose() << endl; -// } -// - for (int i = 0; i < numFactors; i++) { - Pose3 currentPose = results.at(X(i)); - cout << "currentYPREstimate (in Deg): " << currentPose.rotation().ypr().transpose()*180/M_PI << endl; - } -// for (int i = 0; i < numFactors; i++) -// cout << "x" << i << " covariance: " << marginals.marginalCovariance(X(i)).diagonal().transpose() << endl; - - imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.01, 0.0)); + // And compare it with expected value (our prior) + Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); } From 654cb4d31a945d147e1123f0a9a7b072f0efc948 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 26 Jul 2015 07:55:43 +0200 Subject: [PATCH 6/6] Re-factored conversion from sensor to body --- gtsam/navigation/PreintegrationBase.cpp | 31 ++++++++++++++----------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 108063c22..3c29f48c3 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -129,22 +129,27 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc std::pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( const Vector3& measuredAcc, const Vector3& measuredOmega, boost::optional body_P_sensor) const { - Vector3 correctedAcc, correctedOmega; - correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - correctedOmega = biasHat_.correctGyroscope(measuredOmega); + // Correct for bias in the sensor frame + Vector3 s_correctedAcc, s_correctedOmega; + s_correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + s_correctedOmega = biasHat_.correctGyroscope(measuredOmega); - // Then compensate for sensor-body displacement: we express the quantities + // Compensate for sensor-body displacement if needed: we express the quantities // (originally in the IMU frame) into the body frame + // Equations below assume the "body" frame is the CG if (body_P_sensor) { - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - - body_omega_body__cross * body_omega_body__cross - * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - return std::make_pair(correctedAcc, correctedOmega); + Matrix3 bRs = body_P_sensor->rotation().matrix(); + Vector3 b_correctedOmega = bRs * s_correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(b_correctedOmega); + Vector3 b_arm = body_P_sensor->translation().vector(); + Vector3 b_velocity_bs = b_correctedOmega.cross(b_arm); // magnitude: omega * arm + // Subtract out the the centripetal acceleration from the measured one + // to get linear acceleration vector in the body frame: + Vector3 b_correctedAcc = bRs * s_correctedAcc + - b_correctedOmega.cross(b_velocity_bs); + return std::make_pair(b_correctedAcc, b_correctedOmega); + } else + return std::make_pair(correctedAcc, s_correctedOmega); } /// Predict state at time j