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 
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 #if GTSAM_VERSION_NUMERIC >= 40300
67  OptionalJacobian<2,3> H = {}) const;
68 #else
69  OptionalJacobian<2,3> H = boost::none) const;
70 #endif
71 
73 #if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
74  friend class boost::serialization::access;
75  template<class ARCHIVE>
76  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
77  ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
78  ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
79  }
80 #endif
81 };
82 
87 class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
88 
89  typedef NoiseModelFactor1<Rot3> Base;
90 
91 public:
92 
94 #if GTSAM_VERSION_NUMERIC >= 40300
95  typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
96  #else
98 #endif
99 
102 
105  }
106 
107  virtual ~Rot3GravityFactor() {
108  }
109 
118  const Unit3& bRef = Unit3(0, 0, 1)) :
119  Base(model, key), GravityFactor(nZ, bRef) {
120  }
121 
124 #if GTSAM_VERSION_NUMERIC >= 40300
125  return std::static_pointer_cast<gtsam::NonlinearFactor>(
126 #else
127  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
128 #endif
130  }
131 
133  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
134  DefaultKeyFormatter) const;
135 
137  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
138 
140  virtual Vector evaluateError(const Rot3& nRb, //
141 #if GTSAM_VERSION_NUMERIC >= 40300
142  OptionalMatrixType H = OptionalNone) const {
143 #else
144  boost::optional<Matrix&> H = boost::none) const {
145 #endif
146  return attitudeError(nRb, H);
147  }
148  Unit3 nZ() const {
149  return nZ_;
150  }
151  Unit3 bRef() const {
152  return bRef_;
153  }
154 
155 private:
156 #if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
157 
158  friend class boost::serialization::access;
159  template<class ARCHIVE>
160  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
161  ar & boost::serialization::make_nvp("NoiseModelFactor1",
162  boost::serialization::base_object<Base>(*this));
163  ar & boost::serialization::make_nvp("GravityFactor",
164  boost::serialization::base_object<GravityFactor>(*this));
165  }
166 #endif
167 
168 public:
170 };
171 
172 
178  public GravityFactor {
179 
180  typedef NoiseModelFactor1<Pose3> Base;
181 
182 public:
183 
185 #if GTSAM_VERSION_NUMERIC >= 40300
186  typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
187 #else
189 #endif
190  typedef Pose3GravityFactor This;
192 
195  }
196 
198  }
199 
208  const Unit3& bRef = Unit3(0, 0, 1)) :
209  Base(model, key), GravityFactor(nZ, bRef) {
210  }
211 
214 #if GTSAM_VERSION_NUMERIC >= 40300
215  return std::static_pointer_cast<gtsam::NonlinearFactor>(
216 #else
217  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
218 #endif
220  }
221 
223  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
224  DefaultKeyFormatter) const;
225 
227  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
228 
230  virtual Vector evaluateError(const Pose3& nTb, //
231 #if GTSAM_VERSION_NUMERIC >= 40300
232  OptionalMatrixType H = OptionalNone) const {
233 #else
234  boost::optional<Matrix&> H = boost::none) const {
235 #endif
236  Vector e = attitudeError(nTb.rotation(), H);
237  if (H) {
238  Matrix H23 = *H;
239  *H = Matrix::Zero(2,6);
240  H->block<2,3>(0,0) = H23;
241  }
242  return e;
243  }
244  Unit3 nZ() const {
245  return nZ_;
246  }
247  Unit3 bRef() const {
248  return bRef_;
249  }
250 
251 private:
252 #if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
253 
254  friend class boost::serialization::access;
255  template<class ARCHIVE>
256  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
257  ar & boost::serialization::make_nvp("NoiseModelFactor1",
258  boost::serialization::base_object<Base>(*this));
259  ar & boost::serialization::make_nvp("GravityFactor",
260  boost::serialization::base_object<GravityFactor>(*this));
261  }
262 #endif
263 public:
265 };
266 
267 }
268 
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[ *:*] noreverse nowriteback set trange[ *:*] noreverse nowriteback set urange[ *:*] noreverse nowriteback set vrange[ *:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Pose3.h
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
rtabmap::GravityFactor::nZ_
const Unit3 nZ_
Definition: GravityFactor.h:47
Unit3.h
rtabmap::Rot3GravityFactor
Definition: GravityFactor.h:87
boost::shared_ptr
rtabmap::Pose3GravityFactor::~Pose3GravityFactor
virtual ~Pose3GravityFactor()
Definition: GravityFactor.h:197
rtabmap::Rot3GravityFactor::Rot3GravityFactor
Rot3GravityFactor()
Definition: GravityFactor.h:104
rtabmap::Pose3GravityFactor::Base
NoiseModelFactor1< Pose3 > Base
Definition: GravityFactor.h:180
rtabmap::Rot3GravityFactor::nZ
Unit3 nZ() const
Definition: GravityFactor.h:148
rtabmap::GravityFactor::GravityFactor
GravityFactor()
Definition: GravityFactor.h:52
rtabmap::Pose3GravityFactor::serialize
void serialize(ARCHIVE &ar, const unsigned int)
Definition: GravityFactor.h:256
rtabmap::Rot3GravityFactor::evaluateError
virtual Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const
Definition: GravityFactor.h:140
rtabmap::Rot3GravityFactor::Base
NoiseModelFactor1< Rot3 > Base
Definition: GravityFactor.h:89
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
rtabmap::Rot3GravityFactor::Rot3GravityFactor
Rot3GravityFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: GravityFactor.h:117
rtabmap::Rot3GravityFactor::This
Rot3GravityFactor This
Typedef to this class.
Definition: GravityFactor.h:101
gtsam::print
GTSAM_EXPORT void print(const Errors &e, const std::string &s="Errors")
rtabmap::Rot3GravityFactor::clone
virtual gtsam::NonlinearFactor::shared_ptr clone() const
Definition: GravityFactor.h:123
NoiseModelFactor1
#define NoiseModelFactor1
gtsam::Rot3
NonlinearFactor.h
KeyFormatter
std::function< std::string(Key)> KeyFormatter
gtsam::Pose3
rtabmap::Pose3GravityFactor::evaluateError
virtual Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const
Definition: GravityFactor.h:230
rtabmap::Pose3GravityFactor::clone
virtual gtsam::NonlinearFactor::shared_ptr clone() const
Definition: GravityFactor.h:213
rtabmap::GravityFactor
Definition: GravityFactor.h:43
SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
rtabmap::Rot3GravityFactor::~Rot3GravityFactor
virtual ~Rot3GravityFactor()
Definition: GravityFactor.h:107
rtabmap::Rot3GravityFactor::serialize
void serialize(ARCHIVE &ar, const unsigned int)
Definition: GravityFactor.h:160
rtabmap::Rot3GravityFactor::shared_ptr
boost::shared_ptr< Rot3GravityFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: GravityFactor.h:97
gtsam::equals
key
const gtsam::Symbol key( 'X', 0)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
gtsam
rtabmap::GravityFactor::serialize
void serialize(ARCHIVE &ar, const unsigned int)
Definition: GravityFactor.h:76
nRb
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1))
rtabmap::Pose3GravityFactor::shared_ptr
boost::shared_ptr< Pose3GravityFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: GravityFactor.h:188
gtsam::OptionalJacobian
rtabmap::Pose3GravityFactor::bRef
Unit3 bRef() const
Definition: GravityFactor.h:247
gtsam::NonlinearFactor
rtabmap::Pose3GravityFactor
Definition: GravityFactor.h:177
rtabmap::Rot3GravityFactor::bRef
Unit3 bRef() const
Definition: GravityFactor.h:151
gtsam::tol
const G double tol
Eigen::Matrix< double, Eigen::Dynamic, 1 >
rtabmap::GravityFactor::GravityFactor
GravityFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: GravityFactor.h:60
gtsam::Unit3
rtabmap::Pose3GravityFactor::Pose3GravityFactor
Pose3GravityFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: GravityFactor.h:207
Key
std::uint64_t Key
rtabmap
Definition: CameraARCore.cpp:35
trace.model
model
Definition: trace.py:4
rtabmap::Pose3GravityFactor::nZ
Unit3 nZ() const
Definition: GravityFactor.h:244
rtabmap::Pose3GravityFactor::Pose3GravityFactor
Pose3GravityFactor()
Definition: GravityFactor.h:194


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11