diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index d27e5d714..21c4200a9 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -31,22 +31,21 @@ using namespace std; //------------------------------------------------------------------------------ // Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ -void PreintegratedCombinedMeasurements::print( - const string& s) const { - PreintegrationBase::print(s); +void PreintegratedCombinedMeasurements::print(const string& s) const { + PreintegrationType::print(s); cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedCombinedMeasurements::equals( const PreintegratedCombinedMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) && - equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); + return PreintegrationType::equals(other, tol) + && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() { void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements. - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF @@ -80,7 +79,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // Single Jacobians to propagate covariance // TODO(frank): should we not also account for bias on position? - Matrix3 theta_H_biasOmega = - C.topRows<3>(); + Matrix3 theta_H_biasOmega = -C.topRows<3>(); Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); // overall Jacobian wrt preintegrated measurements (df/dx) @@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // BLOCK DIAGONAL TERMS D_t_t(&G_measCov_Gt) = dt * iCov; - D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc * - (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) * - (vel_H_biasAcc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega * - (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * - (theta_H_biasOmega.transpose()); + D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc + * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) + * (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega + * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) + * (theta_H_biasOmega.transpose()); D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) * - theta_H_biasOmega.transpose(); + Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) + * theta_H_biasOmega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; @@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration) { if (!use2ndOrderIntegration) - throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; @@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor( - Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const PreintegratedCombinedMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) {} +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, + Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), _PIM_(pim) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -195,8 +194,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, + Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); // if we need the jacobians @@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor( const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) { +: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), +_PIM_(pim) { boost::shared_ptr p = - boost::make_shared(pim.p()); + boost::make_shared(pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; @@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor( } void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, - CombinedPreintegratedMeasurements& pim, - const Vector3& n_gravity, - const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { // use deprecated predict PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, omegaCoriolis, use2ndOrderCoriolis); @@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, } #endif -} /// namespace gtsam +} + /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3141f8245..bcad9d8f7 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -22,12 +22,19 @@ #pragma once /* GTSAM includes */ +#include +#include #include -#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -57,7 +64,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedCombinedMeasurements : public PreintegrationBase { +class PreintegratedCombinedMeasurements : public PreintegrationType { public: @@ -123,7 +130,7 @@ public: PreintegratedCombinedMeasurements( const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) - : PreintegrationBase(p, biasHat) { + : PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } @@ -133,10 +140,10 @@ public: /// @{ /// Re-initialize PreintegratedCombinedMeasurements - void resetIntegration(); + void resetIntegration() override; /// const reference to params, shadows definition in base class - Params& p() const { return *boost::static_pointer_cast(p_);} + Params& p() const { return *boost::static_pointer_cast(this->p_);} /// @} /// @name Access instance variables @@ -146,7 +153,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; /// @} @@ -163,7 +170,7 @@ public: * frame) */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT); + const Vector3& measuredOmega, const double dt) override; /// @} @@ -183,7 +190,7 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index ea39368d6..7210f4dd2 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -32,20 +32,20 @@ using namespace std; // Inner class PreintegratedImuMeasurements //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::print(const string& s) const { - PreintegrationBase::print(s); + PreintegrationType::print(s); cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedImuMeasurements::equals( const PreintegratedImuMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) + return PreintegrationType::equals(other, tol) && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() { void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements (also get Jacobian) - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a @@ -73,30 +73,31 @@ void PreintegratedImuMeasurements::integrateMeasurement( preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 - preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt; + preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt; } //------------------------------------------------------------------------------ -void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs, - const Matrix& measuredOmegas, - const Matrix& dts) { - assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); +void PreintegratedImuMeasurements::integrateMeasurements( + const Matrix& measuredAccs, const Matrix& measuredOmegas, + const Matrix& dts) { + assert( + measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); assert(dts.cols() >= 1); assert(measuredAccs.cols() == dts.cols()); assert(measuredOmegas.cols() == dts.cols()); size_t n = static_cast(dts.cols()); for (size_t j = 0; j < n; j++) { - integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j)); + integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j)); } } //------------------------------------------------------------------------------ #ifdef GTSAM_TANGENT_PREINTEGRATION -void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // - Matrix9* H1, Matrix9* H2) { - PreintegrationBase::mergeWith(pim12, H1, H2); +void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // + Matrix9* H1, Matrix9* H2) { + PreintegrationType::mergeWith(pim12, H1, H2); preintMeasCov_ = - *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); + *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); } #endif //------------------------------------------------------------------------------ @@ -180,12 +181,12 @@ PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { if (!pim01.matchesParamsWith(pim12)) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with different params"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with different params"); if (pim01.params()->body_P_sensor) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); // the bias for the merged factor will be the bias from 01 PreintegratedImuMeasurements pim02 = pim01; @@ -198,25 +199,25 @@ PreintegratedImuMeasurements ImuFactor::Merge( //------------------------------------------------------------------------------ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, - const shared_ptr& f12) { + const shared_ptr& f12) { // IMU bias keys must be the same. if (f01->key5() != f12->key5()) - throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); + throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); // expect intermediate pose, velocity keys to matchup. if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) - throw std::domain_error( - "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); + throw std::domain_error( + "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); // return new factor auto pim02 = - Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); - return boost::make_shared(f01->key1(), // P0 - f01->key2(), // V0 - f12->key3(), // P2 - f12->key4(), // V2 - f01->key5(), // B - pim02); + Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); + return boost::make_shared(f01->key1(),// P0 + f01->key2(),// V0 + f12->key3(),// P2 + f12->key4(),// V2 + f01->key5(),// B + pim02); } #endif @@ -251,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, //------------------------------------------------------------------------------ // ImuFactor2 methods //------------------------------------------------------------------------------ -ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), - _PIM_(pim) {} +ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, + bias), _PIM_(pim) { +} //------------------------------------------------------------------------------ NonlinearFactor::shared_ptr ImuFactor2::clone() const { @@ -269,9 +272,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { } //------------------------------------------------------------------------------ -void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) - << "," << keyFormatter(this->key3()) << ")\n"; +void ImuFactor2::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) + << ")\n"; cout << *this << endl; } @@ -284,15 +289,15 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, - const imuBias::ConstantBias& bias_i, // - boost::optional H1, - boost::optional H2, - boost::optional H3) const { +Vector ImuFactor2::evaluateError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1, boost::optional H2, + boost::optional H3) const { return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); } //------------------------------------------------------------------------------ } - // namespace gtsam +// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index f3bfd8c83..532abdac0 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -23,11 +23,18 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating @@ -61,7 +68,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedImuMeasurements: public PreintegrationBase { +class PreintegratedImuMeasurements: public PreintegrationType { friend class ImuFactor; friend class ImuFactor2; @@ -85,29 +92,28 @@ public: */ PreintegratedImuMeasurements(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : - PreintegrationBase(p, biasHat) { + PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } /** * Construct preintegrated directly from members: base class and preintMeasCov - * @param base PreintegrationBase instance + * @param base PreintegrationType instance * @param preintMeasCov Covariance matrix used in noise model. */ - PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov) - : PreintegrationBase(base), + PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) + : PreintegrationType(base), preintMeasCov_(preintMeasCov) { } /// print - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; /// equals - bool equals(const PreintegratedImuMeasurements& expected, - double tol = 1e-9) const; + bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; /// Re-initialize PreintegratedIMUMeasurements - void resetIntegration(); + void resetIntegration() override; /** * Add a single IMU measurement to the preintegration. @@ -115,7 +121,8 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt); + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt) override; /// Add multiple measurements, in matrix columns void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, @@ -152,7 +159,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); } }; diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index b9344a524..cc88d9101 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -26,9 +26,9 @@ using namespace std; namespace gtsam { //------------------------------------------------------------------------------ -ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr& p, - const Bias& biasHat) - : PreintegrationBase(p, biasHat) { +ManifoldPreintegration::ManifoldPreintegration( + const boost::shared_ptr& p, const Bias& biasHat) : + PreintegrationBase(p, biasHat) { resetIntegration(); } @@ -46,8 +46,7 @@ void ManifoldPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && deltaXij_.equals(other.deltaXij_, tol) && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) @@ -58,9 +57,9 @@ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, } //------------------------------------------------------------------------------ -void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) { +void ManifoldPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); @@ -83,8 +82,8 @@ void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc, // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } // Update Jacobians @@ -141,4 +140,4 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta( //------------------------------------------------------------------------------ -} // namespace gtsam +}// namespace gtsam diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 60ac7cf63..65ca8f4cc 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -21,7 +21,8 @@ #pragma once -#include +#include +#include namespace gtsam { @@ -72,14 +73,14 @@ public: /// @name Instance variables access /// @{ - const NavState& deltaXij() const override { return deltaXij_; } - const Rot3& deltaRij() const override { return deltaXij_.attitude(); } - Vector3 deltaPij() const override { return deltaXij_.position().vector(); } - Vector3 deltaVij() const override { return deltaXij_.velocity(); } + NavState deltaXij() const override { return deltaXij_; } + Rot3 deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position().vector(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } /// @name Testable /// @{ - bool equals(const ManifoldPreintegration& other, double tol) const override; + bool equals(const ManifoldPreintegration& other, double tol) const; /// @} /// @name Main functionality @@ -89,8 +90,8 @@ public: /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose /// NOTE(frank): implementation is different in two versions - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) override; + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 146fdf8b7..d99c86952 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -31,7 +31,6 @@ namespace gtsam { PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, const Bias& biasHat) : p_(p), biasHat_(biasHat), deltaTij_(0.0) { - resetIntegration(); } //------------------------------------------------------------------------------ @@ -96,6 +95,16 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( return make_pair(correctedAcc, correctedOmega); } +//------------------------------------------------------------------------------ +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt) { + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, + // even when not of interest to the caller. Provide scratch space here. + Matrix9 A; + Matrix93 B, C; + update(measuredAcc, measuredOmega, dt, &A, &B, &C); +} + //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 04539e96b..08dcd1381 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -124,9 +124,9 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } - virtual Vector3 deltaPij() const=0; - virtual Vector3 deltaVij() const=0; - virtual Rot3 deltaRij() const=0; + virtual Vector3 deltaPij() const=0; + virtual Vector3 deltaVij() const=0; + virtual Rot3 deltaRij() const=0; virtual NavState deltaXij() const=0; // Exposed for MATLAB @@ -136,8 +136,7 @@ public: /// @name Testable /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); - void print(const std::string& s) const; - virtual bool equals(const PreintegrationBase& other, double tol) const = 0; + virtual void print(const std::string& s) const; /// @} /// @name Main functionality @@ -155,9 +154,13 @@ public: /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose - virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, + virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0; + /// Version without derivatives + virtual void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt); + /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, @@ -182,11 +185,6 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; - /** Dummy clone for MATLAB */ - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(); - } - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index b7e62b528..2df958d05 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -25,8 +25,8 @@ namespace gtsam { //------------------------------------------------------------------------------ TangentPreintegration::TangentPreintegration(const boost::shared_ptr& p, - const Bias& biasHat) - : TangentPreintegration(p,biasHat) { + const Bias& biasHat) : + PreintegrationBase(p, biasHat) { resetIntegration(); } @@ -41,20 +41,21 @@ void TangentPreintegration::resetIntegration() { //------------------------------------------------------------------------------ bool TangentPreintegration::equals(const TangentPreintegration& other, double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) - && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) - && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); + && equal_with_abs_tol(preintegrated_H_biasAcc_, + other.preintegrated_H_biasAcc_, tol) + && equal_with_abs_tol(preintegrated_H_biasOmega_, + other.preintegrated_H_biasOmega_, tol); } //------------------------------------------------------------------------------ // See extensive discussion in ImuFactor.lyx -Vector9 TangentPreintegration::UpdatePreintegrated( - const Vector3& a_body, const Vector3& w_body, double dt, - const Vector9& preintegrated, OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { +Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, double dt, const Vector9& preintegrated, + OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B, + OptionalJacobian<9, 3> C) { const auto theta = preintegrated.segment<3>(0); const auto position = preintegrated.segment<3>(3); const auto velocity = preintegrated.segment<3>(6); @@ -65,27 +66,27 @@ Vector9 TangentPreintegration::UpdatePreintegrated( // Calculate exact mean propagation Matrix3 w_tangent_H_theta, invH; - const Vector3 w_tangent = // angular velocity mapped back to tangent space + const Vector3 w_tangent = // angular velocity mapped back to tangent space local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); const SO3 R = local.expmap(); const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent* dt, // theta - position + velocity* dt + a_nav* dt22, // position - velocity + a_nav* dt; // velocity + preintegratedPlus << // new preintegrated vector: + theta + w_tangent * dt, // theta + position + velocity * dt + a_nav * dt22, // position + velocity + a_nav * dt; // velocity if (A) { // Exact derivative of R*a with respect to theta: const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta } if (B) { B->block<3, 3>(0, 0) = Z_3x3; @@ -102,10 +103,9 @@ Vector9 TangentPreintegration::UpdatePreintegrated( } //------------------------------------------------------------------------------ -void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - const double dt, Matrix9* A, - Matrix93* B, Matrix93* C) { +void TangentPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); Vector3 omega = biasHat_.correctGyroscope(measuredOmega); @@ -124,8 +124,8 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias @@ -135,24 +135,15 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); } -void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt) { - // NOTE(frank): integrateMeasurement always needs to compute the derivatives, - // even when not of interest to the caller. Provide scratch space here. - Matrix9 A; - Matrix93 B, C; - integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -} //------------------------------------------------------------------------------ Vector9 TangentPreintegration::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { // We correct for a change between bias_i and the biasHat_ used to integrate // This is a simple linear correction with obvious derivatives const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - const Vector9 biasCorrected = - preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + - preintegrated_H_biasOmega_ * biasIncr.gyroscope(); + const Vector9 biasCorrected = preintegrated() + + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + + preintegrated_H_biasOmega_ * biasIncr.gyroscope(); if (H) { (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; @@ -174,9 +165,8 @@ Vector9 TangentPreintegration::biasCorrectedDelta( //------------------------------------------------------------------------------ Vector9 TangentPreintegration::Compose(const Vector9& zeta01, - const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) { + const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { const auto t01 = zeta01.segment<3>(0); const auto p01 = zeta01.segment<3>(3); const auto v01 = zeta01.segment<3>(6); @@ -195,9 +185,9 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01, Matrix3 t02_H_R02; Vector9 zeta02; const Matrix3 R = R01.matrix(); - zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta - p01 + v01 * deltaT12 + R * p12, // position - v01 + R * v12; // velocity + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity if (H1) { H1->setIdentity(); @@ -218,14 +208,16 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01, } //------------------------------------------------------------------------------ -void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix9* H1, - Matrix9* H2) { +void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, + Matrix9* H1, Matrix9* H2) { if (!matchesParamsWith(pim12)) { - throw std::domain_error("Cannot merge pre-integrated measurements with different params"); + throw std::domain_error( + "Cannot merge pre-integrated measurements with different params"); } if (params()->body_P_sensor) { - throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); + throw std::domain_error( + "Cannot merge pre-integrated measurements with sensor pose yet"); } const double t01 = deltaTij(); @@ -241,13 +233,13 @@ void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2); - preintegrated_H_biasAcc_ = - (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; + preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_ + + (*H2) * pim12.preintegrated_H_biasAcc_; - preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + - (*H2) * pim12.preintegrated_H_biasOmega_; + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; } //------------------------------------------------------------------------------ -} // namespace gtsam +}// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 2d44be3ab..7f3aed303 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -64,9 +64,9 @@ public: /// @name Instance variables access /// @{ - Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } - Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } - Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } + Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } + Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } NavState deltaXij() const override { return NavState::Retract(preintegrated_); } const Vector9& preintegrated() const { return preintegrated_; } @@ -76,7 +76,7 @@ public: /// @name Testable /// @{ - bool equals(const TangentPreintegration& other, double tol) const override; + bool equals(const TangentPreintegration& other, double tol) const; /// @} /// @name Main functionality @@ -91,15 +91,12 @@ public: OptionalJacobian<9, 3> B = boost::none, OptionalJacobian<9, 3> C = boost::none); - // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt); - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose /// NOTE(frank): implementation is different in two versions - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, - Matrix9* A, Matrix93* B, Matrix93* C) override; + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override; /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far