From 9bcdf972f87d220d9ea4752f345c98b7331931c6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 24 Jul 2015 14:17:46 +0200 Subject: [PATCH] Asserting that computeError is just localCoordinates --- gtsam/navigation/PreintegrationBase.cpp | 6 +-- gtsam/navigation/PreintegrationBase.h | 3 ++ gtsam/navigation/tests/testImuFactor.cpp | 69 +++++++++++++----------- 3 files changed, 44 insertions(+), 34 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f45b0f49c..e4159e9d7 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -175,8 +175,8 @@ NavState PreintegrationBase::predict(const NavState& state_i, // TODO(frank): this is *almost* state_j.localCoordinates(predict), // except for the damn Ri.transpose. Ri is also the only way this depends on state_i. // That is not an accident! Put R in computed covariances instead ? -static Vector9 computeError(const NavState& state_i, const NavState& state_j, - const NavState& predictedState_j) { +Vector9 PreintegrationBase::computeError(const NavState& state_i, + const NavState& state_j, const NavState& predictedState_j) { const Rot3& rot_i = state_i.attitude(); const Matrix Ri = rot_i.matrix(); @@ -198,7 +198,7 @@ static Vector9 computeError(const NavState& state_i, const NavState& state_j, Vector9 r; r << fR, fp, fv; return r; - // return state_j.localCoordinates(predictedState_j); +// return state_j.localCoordinates(predictedState_j); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 8182693ed..50754636a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -191,6 +191,9 @@ public: OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; + static Vector9 computeError(const NavState& state_i, const NavState& state_j, + const NavState& predictedState_j); + /// 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 f433c2a9e..6419cc079 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -230,7 +230,8 @@ static const NavState state1(x1, v1); // Measurements static const double w = M_PI / 100; static const Vector3 measuredOmega(w, 0, 0); -static const Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown); +static const Vector3 measuredAcc = x1.rotation().unrotate( + -kGravityAlongNavZDown); static const double deltaT = 1.0; static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0), @@ -254,27 +255,32 @@ TEST(ImuFactor, PreintegrationBaseMethods) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - { // biasCorrectedDelta - Matrix96 actualH; - pim.biasCorrectedDelta(bias, actualH); - Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), bias); - EXPECT(assert_equal(expectedH, actualH)); - } - { - Matrix9 aH1; - Matrix96 aH2; - pim.predict(state1, bias, aH1, aH2); - Matrix eH1 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none, - boost::none), state1); - EXPECT(assert_equal(eH1, aH1, 1e-8)); - Matrix eH2 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, - boost::none), bias); - EXPECT(assert_equal(eH2, aH2, 1e-8)); - } + // biasCorrectedDelta + Matrix96 actualH; + pim.biasCorrectedDelta(bias, actualH); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), bias); + EXPECT(assert_equal(expectedH, actualH)); + + Matrix9 aH1; + Matrix96 aH2; + NavState predictedState = pim.predict(state1, bias, aH1, aH2); + Matrix eH1 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none, + boost::none), state1); + EXPECT(assert_equal(eH1, aH1, 1e-8)); + Matrix eH2 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, + boost::none), bias); + EXPECT(assert_equal(eH2, aH2, 1e-8)); + + EXPECT( + assert_equal(Vector(-state1.localCoordinates(predictedState)), + PreintegratedImuMeasurements::computeError(state1, state1, + predictedState), 1e-8)); + return; + } /* ************************************************************************* */ @@ -335,7 +341,7 @@ TEST(ImuFactor, ErrorAndJacobians) { Matrix cov = pim.preintMeasCov(); Matrix R = RtR(cov.inverse()); Vector whitened = R * expectedError; - EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values),1e-5)); + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-5)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-5); @@ -357,10 +363,9 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - PreintegratedImuMeasurements pim( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance); + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); + PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -858,8 +863,8 @@ TEST(ImuFactor, PredictPositionAndVelocity) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravityAlongNavZDown, - kZeroOmegaCoriolis); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, + kGravityAlongNavZDown, kZeroOmegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 1, 0; @@ -894,7 +899,8 @@ TEST(ImuFactor, PredictRotation) { Pose3 x1, x2; Vector3 v1 = Vector3(0, 0.0, 0.0); Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 0, 0; @@ -927,7 +933,8 @@ TEST(ImuFactor, PredictArbitrary) { Pose3 x1, x2; Vector3 v1 = Vector3(0, 0.0, 0.0); Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); // Regression test for Imu Refactor Rot3 expectedR( //