AttitudeFactor.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 
18 #pragma once
19 
20 #include <gtsam/geometry/Pose3.h>
21 #include <gtsam/geometry/Unit3.h>
23 
24 namespace gtsam {
25 
46  protected:
48 
49  public:
52 
59  AttitudeFactor(const Unit3& nRef, const Unit3& bMeasured = Unit3(0, 0, 1))
61 
63  Vector attitudeError(const Rot3& p, OptionalJacobian<2, 3> H = {}) const;
64 
65  const Unit3& nRef() const { return nRef_; }
66  const Unit3& bMeasured() const { return bMeasured_; }
67 
68 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
69  [[deprecated("Use nRef() instead")]]
70  const Unit3& nZ() const { return nRef_; }
71 
72  [[deprecated("Use bMeasured() instead")]]
73  const Unit3& bRef() const { return bMeasured_; }
74 #endif
75 
76 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
77 
78  friend class boost::serialization::access;
79  template <class ARCHIVE>
80  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
81  ar& boost::serialization::make_nvp("nRef_", nRef_);
82  ar& boost::serialization::make_nvp("bMeasured_", bMeasured_);
83  }
84 #endif
85 };
86 
91 class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN<Rot3>,
92  public AttitudeFactor {
94 
95  public:
96  // Provide access to the Matrix& version of evaluateError:
97  using Base::evaluateError;
98 
100  typedef std::shared_ptr<Rot3AttitudeFactor> shared_ptr;
101 
104 
107 
108  ~Rot3AttitudeFactor() override {}
109 
118  const Unit3& bMeasured = Unit3(0, 0, 1))
119  : Base(model, key), AttitudeFactor(nRef, bMeasured) {}
120 
123  return std::static_pointer_cast<gtsam::NonlinearFactor>(
125  }
126 
128  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
129  DefaultKeyFormatter) const override;
130 
132  bool equals(const NonlinearFactor& expected,
133  double tol = 1e-9) const override;
134 
136  Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
137  return attitudeError(nRb, H);
138  }
139 
140  private:
141 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
142 
143  friend class boost::serialization::access;
144  template <class ARCHIVE>
145  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
146  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
147  ar& boost::serialization::make_nvp(
148  "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
149  ar& boost::serialization::make_nvp(
150  "AttitudeFactor",
151  boost::serialization::base_object<AttitudeFactor>(*this));
152  }
153 #endif
154 
155  public:
157 };
158 
160 template <>
161 struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
162 
167 class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN<Pose3>,
168  public AttitudeFactor {
170 
171  public:
172  // Provide access to the Matrix& version of evaluateError:
173  using Base::evaluateError;
174 
176  typedef std::shared_ptr<Pose3AttitudeFactor> shared_ptr;
177 
180 
183 
184  ~Pose3AttitudeFactor() override {}
185 
194  const Unit3& bMeasured = Unit3(0, 0, 1))
195  : Base(model, key), AttitudeFactor(nRef, bMeasured) {}
196 
199  return std::static_pointer_cast<gtsam::NonlinearFactor>(
201  }
202 
204  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
205  DefaultKeyFormatter) const override;
206 
208  bool equals(const NonlinearFactor& expected,
209  double tol = 1e-9) const override;
210 
212  Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override {
213  Vector e = attitudeError(nTb.rotation(), H);
214  if (H) {
215  Matrix H23 = *H;
216  *H = Matrix::Zero(2, 6);
217  H->block<2, 3>(0, 0) = H23;
218  }
219  return e;
220  }
221 
222  private:
223 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
224 
225  friend class boost::serialization::access;
226  template <class ARCHIVE>
227  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
228  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
229  ar& boost::serialization::make_nvp(
230  "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
231  ar& boost::serialization::make_nvp(
232  "AttitudeFactor",
233  boost::serialization::base_object<AttitudeFactor>(*this));
234  }
235 #endif
236 
237  public:
239 };
240 
242 template <>
243 struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
244 
245 } // namespace gtsam
gtsam::Pose3AttitudeFactor::~Pose3AttitudeFactor
~Pose3AttitudeFactor() override
Definition: AttitudeFactor.h:184
gtsam::AttitudeFactor::AttitudeFactor
AttitudeFactor()
Definition: AttitudeFactor.h:51
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
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::Pose3AttitudeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: AttitudeFactor.h:198
gtsam::Pose3AttitudeFactor::Base
NoiseModelFactorN< Pose3 > Base
Definition: AttitudeFactor.h:169
gtsam::Pose3AttitudeFactor::This
Pose3AttitudeFactor This
Typedef to this class.
Definition: AttitudeFactor.h:179
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Rot3AttitudeFactor::shared_ptr
std::shared_ptr< Rot3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: AttitudeFactor.h:100
gtsam::Rot3AttitudeFactor::Base
NoiseModelFactorN< Rot3 > Base
Definition: AttitudeFactor.h:93
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::AttitudeFactor::attitudeError
Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H={}) const
Definition: AttitudeFactor.cpp:26
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
Unit3.h
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
gtsam::Rot3AttitudeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: AttitudeFactor.h:122
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
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::Rot3AttitudeFactor
Definition: AttitudeFactor.h:91
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::AttitudeFactor
Base class for an attitude factor that constrains the rotation between body and navigation frames.
Definition: AttitudeFactor.h:45
gtsam::AttitudeFactor::nRef
const Unit3 & nRef() const
Definition: AttitudeFactor.h:65
gtsam::Pose3AttitudeFactor
Definition: AttitudeFactor.h:167
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::AttitudeFactor::bMeasured
const Unit3 & bMeasured() const
Definition: AttitudeFactor.h:66
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::Rot3AttitudeFactor::Rot3AttitudeFactor
Rot3AttitudeFactor()
Definition: AttitudeFactor.h:106
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::equals
Definition: Testable.h:112
key
const gtsam::Symbol key('X', 0)
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Pose3AttitudeFactor::Pose3AttitudeFactor
Pose3AttitudeFactor(Key key, const Unit3 &nRef, const SharedNoiseModel &model, const Unit3 &bMeasured=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:193
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:316
gtsam::Pose3AttitudeFactor::evaluateError
Vector evaluateError(const Pose3 &nTb, OptionalMatrixType H) const override
Definition: AttitudeFactor.h:212
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
gtsam::Rot3AttitudeFactor::~Rot3AttitudeFactor
~Rot3AttitudeFactor() override
Definition: AttitudeFactor.h:108
gtsam::Rot3AttitudeFactor::Rot3AttitudeFactor
Rot3AttitudeFactor(Key key, const Unit3 &nRef, const SharedNoiseModel &model, const Unit3 &bMeasured=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:117
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Rot3AttitudeFactor::This
Rot3AttitudeFactor This
Typedef to this class.
Definition: AttitudeFactor.h:103
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::AttitudeFactor::bMeasured_
Unit3 bMeasured_
Position measurement in.
Definition: AttitudeFactor.h:47
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
gtsam::AttitudeFactor::nRef_
Unit3 nRef_
Definition: AttitudeFactor.h:47
gtsam::Pose3AttitudeFactor::Pose3AttitudeFactor
Pose3AttitudeFactor()
Definition: AttitudeFactor.h:182
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::AttitudeFactor::AttitudeFactor
AttitudeFactor(const Unit3 &nRef, const Unit3 &bMeasured=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:59
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Pose3AttitudeFactor::shared_ptr
std::shared_ptr< Pose3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: AttitudeFactor.h:176
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
gtsam::Rot3AttitudeFactor::evaluateError
Vector evaluateError(const Rot3 &nRb, OptionalMatrixType H) const override
Definition: AttitudeFactor.h:136


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:09