From 7b60c5029777b373d7d6cf0814a8f452d0c050de Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 27 Jan 2016 16:02:40 -0800 Subject: [PATCH] New method computeError, and more derivative checking (though, expression factors already checked out) --- gtsam/navigation/PreintegrationBase.cpp | 47 +++++++++++++------ gtsam/navigation/PreintegrationBase.h | 6 +++ gtsam/navigation/tests/testImuFactor.cpp | 29 +++++++++--- .../tests/testPreintegrationBase.cpp | 18 +++++++ python/handwritten/navigation/ImuFactor.cpp | 1 + 5 files changed, 81 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 639ceb574..a26498500 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -302,6 +302,32 @@ NavState PreintegrationBase::predict(const NavState& state_i, return state_j; } +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3) const { + // Predict state at time j + Matrix9 D_predict_state_i; + Matrix96 D_predict_bias_i; + NavState predictedState_j = predict( + state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0); + + // Calculate error + Matrix9 D_error_state_j, D_error_predict; + Vector9 error = + state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0, + H1 || H3 ? &D_error_predict : 0); + + if (H1) *H1 << D_error_predict* D_predict_state_i; + if (H2) *H2 << D_error_state_j; + if (H3) *H3 << D_error_predict* D_predict_bias_i; + + return error; +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, @@ -314,26 +340,20 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, NavState state_i(pose_i, vel_i); NavState state_j(pose_j, vel_j); - /// Predict state at time j - Matrix99 D_predict_state_i; - Matrix96 D_predict_bias_i; - NavState predictedState_j = predict(state_i, bias_i, - H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0); - - Matrix9 D_error_state_j, D_error_predict; - Vector9 error = state_j.localCoordinates(predictedState_j, - H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0); + // Predict state at time j + Matrix9 D_error_state_i, D_error_state_j; + Vector9 error = computeError(state_i, state_j, bias_i, + H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5); // Separate out derivatives in terms of 5 arguments // Note that doing so requires special treatment of velocities, as when treated as // separate variables the retract applied will not be the semi-direct product in NavState // Instead, the velocities in nav are updated using a straight addition // This is difference is accounted for by the R().transpose calls below - if (H1) *H1 << D_error_predict* D_predict_state_i.leftCols<6>(); - if (H2) *H2 << D_error_predict* D_predict_state_i.rightCols<3>() * state_i.R().transpose(); + if (H1) *H1 << D_error_state_i.leftCols<6>(); + if (H2) *H2 << D_error_state_i.rightCols<3>() * state_i.R().transpose(); if (H3) *H3 << D_error_state_j.leftCols<6>(); if (H4) *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose(); - if (H5) *H5 << D_error_predict* D_predict_bias_i; return error; } @@ -355,5 +375,4 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, #endif //------------------------------------------------------------------------------ -} - /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3055fe9ed..92f1db8aa 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -264,6 +264,12 @@ public: OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; + /// Calculate error given navStates + Vector9 computeError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3) const; + /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 5602a20ad..e40c9adea 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -71,7 +71,7 @@ static boost::shared_ptr defaultParams() { return p; } -// Auxiliary functions to test preintegrated Jacobians +// Auxiliary functions to test pre-integrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ PreintegratedImuMeasurements evaluatePreintegratedMeasurements( @@ -151,14 +151,14 @@ TEST(ImuFactor, PreintegratedMeasurements) { Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; - // Expected preintegrated values + // Expected pre-integrated values Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; Vector3 expectedDeltaV1(0.05, 0.0, 0.0); Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); - // Actual preintegrated values + // Actual pre-integrated values PreintegratedImuMeasurements actual1(defaultParams()); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -167,6 +167,24 @@ TEST(ImuFactor, PreintegratedMeasurements) { EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()))); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); + // Check derivatives of computeError + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + NavState x1, x2 = actual1.predict(x1, bias); + + { + Matrix9 aH1, aH2; + Matrix96 aH3; + actual1.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&PreintegrationBase::computeError, actual1, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); + } + // Integrate again Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; @@ -175,7 +193,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT2(1); - // Actual preintegrated values + // Actual pre-integrated values PreintegratedImuMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -184,7 +202,6 @@ TEST(ImuFactor, PreintegratedMeasurements) { EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()))); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9); } - /* ************************************************************************* */ // Common linearization point and measurements for tests namespace common { @@ -482,7 +499,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { deltaTs.push_back(0.01); } - // Actual preintegrated values + // Actual pre-integrated values PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, deltaTs); diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index af2aa1f96..5363f6b9f 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -73,6 +73,24 @@ TEST(PreintegrationBase, UpdateEstimate2) { EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); } +/* ************************************************************************* */ +TEST(PreintegrationBase, computeError) { + PreintegrationBase pim(defaultParams()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index b3d6126bd..e8d1e9aaa 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -68,6 +68,7 @@ void exportImuFactor() { const imuBias::ConstantBias&>()) .def(repr(self)) .def("predict", &PreintegratedImuMeasurements::predict) + .def("computeError", &PreintegratedImuMeasurements::computeError) .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) .def("integrateMeasurement", &PreintegratedImuMeasurements::integrateMeasurement)