CombinedImuFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
23 #pragma once
24 
25 /* GTSAM includes */
27 
28 namespace gtsam {
29 
30 #ifdef GTSAM_TANGENT_PREINTEGRATION
31 typedef TangentPreintegration PreintegrationType;
32 #else
34 #endif
35 
36 /*
37  * If you are using the factor, please cite:
38  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
39  * conditionally independent sets in factor graphs: a unifying perspective based
40  * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
41  *
42  * REFERENCES:
43  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
44  * Volume 2, 2008.
45  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
46  * High-Dynamic Motion in Built Environments Without Initial Conditions",
47  * TRO, 28(1):61-76, 2012.
48  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
49  * Computation of the Jacobian Matrices", Tech. Report, 2013.
50  * Available in this repo as "PreintegratedIMUJacobians.pdf".
51  * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
52  * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
53  * Robotics: Science and Systems (RSS), 2015.
54  */
55 
67  : public PreintegrationType {
68  public:
70 
71  protected:
72  /* Covariance matrix of the preintegrated measurements
73  * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc
74  * BiasOmega] (first-order propagation from *measurementCovariance*).
75  * PreintegratedCombinedMeasurements also include the biases and keep the
76  * correlation between the preintegrated measurements and the biases
77  */
79 
80  friend class CombinedImuFactor;
81 
82  public:
85 
88 
96  const std::shared_ptr<Params>& p,
98  const Eigen::Matrix<double, 15, 15>& preintMeasCov =
100  : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {}
101 
109  const PreintegrationType& base,
110  const Eigen::Matrix<double, 15, 15>& preintMeasCov)
111  : PreintegrationType(base), preintMeasCov_(preintMeasCov) {}
112 
115 
117 
120 
122  void resetIntegration() override;
123 
130  void resetIntegration(const gtsam::Matrix6& Q_init);
131 
133  Params& p() const { return *std::static_pointer_cast<Params>(this->p_); }
135 
139  Matrix preintMeasCov() const { return preintMeasCov_; }
141 
145  void print(
146  const std::string& s = "Preintegrated Measurements:") const override;
149  double tol = 1e-9) const;
151 
154 
165  void integrateMeasurement(const Vector3& measuredAcc,
166  const Vector3& measuredOmega,
167  const double dt) override;
168 
170 
171  private:
172 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
173  friend class boost::serialization::access;
175  template <class ARCHIVE>
176  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
177  namespace bs = ::boost::serialization;
178  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
179  ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
180  }
181 #endif
182 
183  public:
185 };
186 
205 class GTSAM_EXPORT CombinedImuFactor
206  : public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
207  imuBias::ConstantBias, imuBias::ConstantBias> {
208  public:
209  private:
214 
216 
217  public:
218  // Provide access to Matrix& version of evaluateError:
219  using Base::evaluateError;
220 
222 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
223  typedef typename std::shared_ptr<CombinedImuFactor> shared_ptr;
224 #else
225  typedef std::shared_ptr<CombinedImuFactor> shared_ptr;
226 #endif
227 
230 
242  Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
243  const PreintegratedCombinedMeasurements& preintegratedMeasurements);
244 
245  ~CombinedImuFactor() override {}
246 
248  gtsam::NonlinearFactor::shared_ptr clone() const override;
249 
252  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
255  const CombinedImuFactor&);
257  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
258  DefaultKeyFormatter) const override;
259 
261  bool equals(const NonlinearFactor& expected,
262  double tol = 1e-9) const override;
264 
268  return _PIM_;
269  }
270 
273  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
275  const Pose3& pose_j, const Vector3& vel_j,
276  const imuBias::ConstantBias& bias_i,
277  const imuBias::ConstantBias& bias_j,
281  OptionalMatrixType H6) const override;
282 
283  private:
284 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
285 
286  friend class boost::serialization::access;
287  template <class ARCHIVE>
288  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
289  // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
290  ar& boost::serialization::make_nvp(
291  "NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
292  ar& BOOST_SERIALIZATION_NVP(_PIM_);
293  }
294 #endif
295 
296  public:
298 };
299 // class CombinedImuFactor
300 
301 template <>
303  : public Testable<PreintegrationCombinedParams> {};
304 
305 template <>
307  : public Testable<PreintegratedCombinedMeasurements> {};
308 
309 template <>
310 struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
311 
312 } // namespace gtsam
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
gtsam::PreintegratedCombinedMeasurements::~PreintegratedCombinedMeasurements
~PreintegratedCombinedMeasurements() override
Virtual destructor.
Definition: CombinedImuFactor.h:114
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::CombinedImuFactor::shared_ptr
std::shared_ptr< CombinedImuFactor > shared_ptr
Definition: CombinedImuFactor.h:225
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias(), const Eigen::Matrix< double, 15, 15 > &preintMeasCov=Eigen::Matrix< double, 15, 15 >::Zero())
Definition: CombinedImuFactor.h:95
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::PreintegratedCombinedMeasurements::Params
PreintegrationCombinedParams Params
Definition: CombinedImuFactor.h:69
os
ofstream os("timeSchurFactors.csv")
gtsam::Factor::shared_ptr
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:76
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::CombinedImuFactor::This
CombinedImuFactor This
Definition: CombinedImuFactor.h:210
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::CombinedImuFactor::CombinedImuFactor
CombinedImuFactor()
Definition: CombinedImuFactor.h:229
PreintegrationCombinedParams.h
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
Definition: CombinedImuFactor.h:108
biased_x_rotation::measuredOmega
const Vector3 measuredOmega
Definition: testPreintegratedRotation.cpp:35
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::PreintegratedCombinedMeasurements::p
Params & p() const
const reference to params, shadows definition in base class
Definition: CombinedImuFactor.h:133
gtsam::PreintegratedCombinedMeasurements::preintMeasCov_
Eigen::Matrix< double, 15, 15 > preintMeasCov_
Definition: CombinedImuFactor.h:78
gtsam::PreintegrationType
ManifoldPreintegration PreintegrationType
Definition: CombinedImuFactor.h:33
gtsam::equals
Definition: Testable.h:112
gtsam::ManifoldPreintegration
Definition: ManifoldPreintegration.h:33
gtsam::CombinedImuFactor
Definition: CombinedImuFactor.h:205
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam
traits
Definition: SFMdata.h:40
gtsam::CombinedImuFactor::~CombinedImuFactor
~CombinedImuFactor() override
Definition: CombinedImuFactor.h:245
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::CombinedImuFactor::Base
NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias, imuBias::ConstantBias > Base
Definition: CombinedImuFactor.h:213
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::PreintegratedCombinedMeasurements::preintMeasCov
Matrix preintMeasCov() const
Definition: CombinedImuFactor.h:139
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::CombinedImuFactor::preintegratedMeasurements
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
Definition: CombinedImuFactor.h:267
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
common::measuredAcc
static const Vector3 measuredAcc
Definition: testImuFactor.cpp:181
Eigen::Matrix< double, 15, 15 >
Eigen::PlainObjectBase::setZero
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Definition: CwiseNullaryOp.h:562
gtsam::PreintegrationCombinedParams
Definition: PreintegrationCombinedParams.h:36
gtsam::PreintegratedCombinedMeasurements
Definition: CombinedImuFactor.h:66
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
Definition: CombinedImuFactor.h:87
gtsam::CombinedImuFactor::_PIM_
PreintegratedCombinedMeasurements _PIM_
Definition: CombinedImuFactor.h:215
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:07