GravityFactor.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 
25 #pragma once
26 
27 #include <gtsam/nonlinear/NonlinearFactor.h>
28 #include <gtsam/geometry/Pose3.h>
29 #include <gtsam/geometry/Unit3.h>
30 
31 using namespace gtsam;
32 
33 namespace rtabmap {
34 
44 
45 protected:
46 
47  const Unit3 nZ_, bRef_;
48 
49 public:
50 
53  }
54 
60  GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
61  nZ_(nZ), bRef_(bRef) {
62  }
63 
65  Vector attitudeError(const Rot3& p,
66  OptionalJacobian<2,3> H = boost::none) const;
67 
69  friend class boost::serialization::access;
70  template<class ARCHIVE>
71  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
72  ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
73  ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
74  }
75 };
76 
81 class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
82 
83  typedef NoiseModelFactor1<Rot3> Base;
84 
85 public:
86 
89 
92 
95  }
96 
97  virtual ~Rot3GravityFactor() {
98  }
99 
107  Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
108  const Unit3& bRef = Unit3(0, 0, 1)) :
109  Base(model, key), GravityFactor(nZ, bRef) {
110  }
111 
113  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
114  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
115  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
116  }
117 
119  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
120  DefaultKeyFormatter) const;
121 
123  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
124 
126  virtual Vector evaluateError(const Rot3& nRb, //
127  boost::optional<Matrix&> H = boost::none) const {
128  return attitudeError(nRb, H);
129  }
130  Unit3 nZ() const {
131  return nZ_;
132  }
133  Unit3 bRef() const {
134  return bRef_;
135  }
136 
137 private:
138 
140  friend class boost::serialization::access;
141  template<class ARCHIVE>
142  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
143  ar & boost::serialization::make_nvp("NoiseModelFactor1",
144  boost::serialization::base_object<Base>(*this));
145  ar & boost::serialization::make_nvp("GravityFactor",
146  boost::serialization::base_object<GravityFactor>(*this));
147  }
148 
149 public:
150  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
151 };
152 
153 
158 class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
159  public GravityFactor {
160 
161  typedef NoiseModelFactor1<Pose3> Base;
162 
163 public:
164 
167 
170 
173  }
174 
176  }
177 
185  Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
186  const Unit3& bRef = Unit3(0, 0, 1)) :
187  Base(model, key), GravityFactor(nZ, bRef) {
188  }
189 
191  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
192  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
193  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
194  }
195 
197  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
198  DefaultKeyFormatter) const;
199 
201  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
202 
204  virtual Vector evaluateError(const Pose3& nTb, //
205  boost::optional<Matrix&> H = boost::none) const {
206  Vector e = attitudeError(nTb.rotation(), H);
207  if (H) {
208  Matrix H23 = *H;
209  *H = Matrix::Zero(2,6);
210  H->block<2,3>(0,0) = H23;
211  }
212  return e;
213  }
214  Unit3 nZ() const {
215  return nZ_;
216  }
217  Unit3 bRef() const {
218  return bRef_;
219  }
220 
221 private:
222 
224  friend class boost::serialization::access;
225  template<class ARCHIVE>
226  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
227  ar & boost::serialization::make_nvp("NoiseModelFactor1",
228  boost::serialization::base_object<Base>(*this));
229  ar & boost::serialization::make_nvp("GravityFactor",
230  boost::serialization::base_object<GravityFactor>(*this));
231  }
232 
233 public:
234  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
235 };
236 
237 }
238 
boost::shared_ptr< Rot3GravityFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: GravityFactor.h:88
void serialize(ARCHIVE &ar, const unsigned int)
GLM_FUNC_DECL genType e()
NoiseModelFactor1< Pose3 > Base
Pose3GravityFactor This
Typedef to this class.
virtual Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix &> H=boost::none) const
void serialize(ARCHIVE &ar, const unsigned int)
ROSCONSOLE_DECL void print(FilterBase *filter, void *logger, Level level, const char *file, int line, const char *function, const char *fmt,...) ROSCONSOLE_PRINTF_ATTRIBUTE(7
PM::Matrix Matrix
virtual gtsam::NonlinearFactor::shared_ptr clone() const
NoiseModelFactor1< Rot3 > Base
Definition: GravityFactor.h:83
boost::shared_ptr< Pose3GravityFactor > shared_ptr
shorthand for a smart pointer to a factor
void serialize(ARCHIVE &ar, const unsigned int)
Definition: GravityFactor.h:71
Rot3GravityFactor This
Typedef to this class.
Definition: GravityFactor.h:91
Rot3GravityFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
PM::Vector Vector
model
Definition: trace.py:4
Pose3GravityFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
virtual gtsam::NonlinearFactor::shared_ptr clone() const
virtual Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix &> H=boost::none) const
GravityFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: GravityFactor.h:60


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28