Everything compiles and runs with derived classes
							parent
							
								
									308a75e49b
								
							
						
					
					
						commit
						cbf062ff32
					
				|  | @ -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<Params> 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<Pose3>& 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<CombinedPreintegratedMeasurements::Params> p = | ||||
|       boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p()); | ||||
|   boost::make_shared<CombinedPreintegratedMeasurements::Params>(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
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -22,12 +22,19 @@ | |||
| #pragma once | ||||
| 
 | ||||
| /* GTSAM includes */ | ||||
| #include <gtsam/navigation/ManifoldPreintegration.h> | ||||
| #include <gtsam/navigation/TangentPreintegration.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/navigation/PreintegrationBase.h> | ||||
| #include <gtsam/base/Matrix.h> | ||||
| 
 | ||||
| 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<Params>& 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<Params>(p_);} | ||||
|   Params& p() const { return *boost::static_pointer_cast<Params>(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 <class ARCHIVE> | ||||
|   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_); | ||||
|   } | ||||
| }; | ||||
|  |  | |||
|  | @ -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<size_t>(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<ImuFactor>(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<ImuFactor>(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<Matrix&> H1, | ||||
|                                  boost::optional<Matrix&> H2, | ||||
|                                  boost::optional<Matrix&> H3) const { | ||||
| Vector ImuFactor2::evaluateError(const NavState& state_i, | ||||
|     const NavState& state_j, | ||||
|     const imuBias::ConstantBias& bias_i, //
 | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, | ||||
|     boost::optional<Matrix&> H3) const { | ||||
|   return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| 
 | ||||
| } | ||||
|  // namespace gtsam
 | ||||
| // namespace gtsam
 | ||||
|  |  | |||
|  | @ -23,11 +23,18 @@ | |||
| 
 | ||||
| /* GTSAM includes */ | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/navigation/PreintegrationBase.h> | ||||
| #include <gtsam/navigation/ManifoldPreintegration.h> | ||||
| #include <gtsam/navigation/TangentPreintegration.h> | ||||
| #include <gtsam/base/debug.h> | ||||
| 
 | ||||
| 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<PreintegrationParams>& 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<class ARCHIVE> | ||||
|   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())); | ||||
|   } | ||||
| }; | ||||
|  |  | |||
|  | @ -26,9 +26,9 @@ using namespace std; | |||
| namespace gtsam { | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr<Params>& p, | ||||
|                                        const Bias& biasHat) | ||||
|     : PreintegrationBase(p, biasHat) { | ||||
| ManifoldPreintegration::ManifoldPreintegration( | ||||
|     const boost::shared_ptr<Params>& 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
 | ||||
|  |  | |||
|  | @ -21,7 +21,8 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/navigation/ManifoldPreintegration.h> | ||||
| #include <gtsam/navigation/NavState.h> | ||||
| #include <gtsam/navigation/PreintegrationBase.h> | ||||
| 
 | ||||
| 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
 | ||||
|  |  | |||
|  | @ -31,7 +31,6 @@ namespace gtsam { | |||
| PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p, | ||||
|                                        const Bias& biasHat) | ||||
|     : p_(p), biasHat_(biasHat), deltaTij_(0.0) { | ||||
|   resetIntegration(); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
|  | @ -96,6 +95,16 @@ pair<Vector3, Vector3> 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, | ||||
|  |  | |||
|  | @ -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<PreintegrationBase> clone() const { | ||||
|     return boost::shared_ptr<PreintegrationBase>(); | ||||
|   } | ||||
| 
 | ||||
| #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 | ||||
|   /// @name Deprecated
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -25,8 +25,8 @@ namespace gtsam { | |||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& 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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue