From d4d99c390d38538ffc1581c262b50391bbdddf5e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 19 Jul 2015 03:31:11 -0700 Subject: [PATCH] A custom retract does the trick --- gtsam/navigation/PreintegrationBase.cpp | 27 +++++++------------ gtsam/navigation/PreintegrationBase.h | 36 ++++++++++++++++++++++++- 2 files changed, 45 insertions(+), 18 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index e39200cea..281e1e176 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -129,10 +129,6 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( } } -static Eigen::Block dR(Vector9& v) { return v.segment<3>(0); } -static Eigen::Block dP(Vector9& v) { return v.segment<3>(3); } -static Eigen::Block dV(Vector9& v) { return v.segment<3>(6); } - //------------------------------------------------------------------------------ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const { Vector9 result = Vector9::Zero(); @@ -143,14 +139,14 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const { const double dt = deltaTij(), dt2 = dt * dt; const Vector3& omegaCoriolis = *p().omegaCoriolis; - dR(result) -= Ri.transpose() * omegaCoriolis * dt; - dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper - dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt; + NavState::dR(result) -= Ri.transpose() * omegaCoriolis * dt; + NavState::dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper + NavState::dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt; if (p().use2ndOrderCoriolis) { Vector3 temp = omegaCoriolis.cross( omegaCoriolis.cross(pose_i.translation().vector())); - dP(result) -= 0.5 * temp * dt2; - dV(result) -= temp * dt; + NavState::dP(result) -= 0.5 * temp * dt2; + NavState::dV(result) -= temp * dt; } } return result; @@ -169,9 +165,9 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i, // Rotation, translation, and velocity: Vector9 delta; - dR(delta) = Rot3::Logmap(deltaRij_biascorrected); - dP(delta) = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2; - dV(delta) = Ri * deltaVij_biascorrected + p().gravity * dt; + NavState::dR(delta) = Rot3::Logmap(deltaRij_biascorrected); + NavState::dP(delta) = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2; + NavState::dV(delta) = Ri * deltaVij_biascorrected + p().gravity * dt; if (p().omegaCoriolis) delta += integrateCoriolis(state_i); return delta; @@ -186,11 +182,7 @@ NavState PreintegrationBase::predict(const NavState& state_i, Vector9 delta = recombinedPrediction(state_i, deltaRij_biascorrected, deltaPij_biascorrected, deltaVij_biascorrected); - // TODO(frank): pose update below is separate expmap for R,t. Is that kosher? - const Pose3& pose_i = state_i.pose(); - const Vector3& vel_i = state_i.velocity(); - const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR(delta)), pose_i.translation() + Point3(dP(delta))); - return NavState(pose_j, vel_i + dV(delta)); + return state_i.retract(delta); } //------------------------------------------------------------------------------ @@ -309,6 +301,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const -delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias -delVdelBiasAcc(), -delVdelBiasOmega(); // dfV/dBias } + // TODO(frank): Vector9 r = state_i.localCoordinates(predictedState_j); does not work ??? Vector9 r; r << fR, fp, fv; return r; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 01df67c69..1226eaa6c 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -36,23 +36,57 @@ typedef Vector3 Velocity3; /** * Navigation state: Pose (rotation, translation) + velocity */ -class NavState: private ProductLieGroup { +class NavState: public ProductLieGroup { protected: typedef ProductLieGroup Base; typedef OptionalJacobian<9, 9> ChartJacobian; + using Base::first; + using Base::second; public: // constructors NavState() {} NavState(const Pose3& pose, const Velocity3& vel) : Base(pose, vel) {} + NavState(const Rot3& rot, const Point3& t, const Velocity3& vel): Base(Pose3(rot, t), vel) {} + NavState(const Base& product) : Base(product) {} // access const Pose3& pose() const { return first; } const Point3& translation() const { return pose().translation(); } const Rot3& rotation() const { return pose().rotation(); } const Velocity3& velocity() const { return second; } + + /// @name Manifold + /// @{ + + // NavState tangent space sugar. + static Eigen::Block dR(Vector9& v) { return v.segment<3>(0); } + static Eigen::Block dP(Vector9& v) { return v.segment<3>(3); } + static Eigen::Block dV(Vector9& v) { return v.segment<3>(6); } + + // Specialize Retract/Local that agrees with IMUFactors + // TODO(frank): This is a very specific retract. Talk to Luca about implications. + NavState retract(Vector9& v, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("NavState::retract derivatives not implemented yet"); + return NavState(rotation().expmap(dR(v)), translation() + Point3(dP(v)), velocity() + dV(v)); + } + Vector9 localCoordinates(const NavState& g, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("NavState::localCoordinates derivatives not implemented yet"); + Vector9 v; + dR(v) = rotation().logmap(g.rotation()); + dP(v) = (g.translation() - translation()).vector(); + dV(v) = g.velocity() - velocity(); + return v; + } + /// @} }; +// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors +template<> +struct traits : internal::LieGroupTraits {}; + /// @deprecated struct PoseVelocityBias { Pose3 pose;