gtsam  4.1.0
gtsam
AttitudeFactor.h
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 
21 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/geometry/Unit3.h>
23 
24 namespace gtsam {
25 
35 
36 protected:
37 
38  Unit3 nZ_, bRef_;
39 
40 public:
41 
44  }
45 
51  AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
52  nZ_(nZ), bRef_(bRef) {
53  }
54 
56  Vector attitudeError(const Rot3& p,
57  OptionalJacobian<2,3> H = boost::none) const;
58 
59  const Unit3& nZ() const {
60  return nZ_;
61  }
62  const Unit3& bRef() const {
63  return bRef_;
64  }
65 
68  template<class ARCHIVE>
69  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
70  ar & boost::serialization::make_nvp("nZ_", nZ_);
71  ar & boost::serialization::make_nvp("bRef_", bRef_);
72  }
73 };
74 
79 class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1<Rot3>, public AttitudeFactor {
80 
82 
83 public:
84 
86  typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
87 
90 
93  }
94 
95  virtual ~Rot3AttitudeFactor() {
96  }
97 
105  Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
106  const Unit3& bRef = Unit3(0, 0, 1)) :
107  Base(model, key), AttitudeFactor(nZ, bRef) {
108  }
109 
111  gtsam::NonlinearFactor::shared_ptr clone() const override {
112  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
113  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
114  }
115 
117  void print(const std::string& s, const KeyFormatter& keyFormatter =
118  DefaultKeyFormatter) const override;
119 
121  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
122 
124  Vector evaluateError(const Rot3& nRb, //
125  boost::optional<Matrix&> H = boost::none) const override {
126  return attitudeError(nRb, H);
127  }
128 
129 private:
130 
132  friend class boost::serialization::access;
133  template<class ARCHIVE>
134  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
135  ar & boost::serialization::make_nvp("NoiseModelFactor1",
136  boost::serialization::base_object<Base>(*this));
137  ar & boost::serialization::make_nvp("AttitudeFactor",
138  boost::serialization::base_object<AttitudeFactor>(*this));
139  }
140 
141 public:
143 };
144 
146 template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
147 
152 class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>,
153  public AttitudeFactor {
154 
156 
157 public:
158 
160  typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
161 
164 
167  }
168 
169  virtual ~Pose3AttitudeFactor() {
170  }
171 
179  Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
180  const Unit3& bRef = Unit3(0, 0, 1)) :
181  Base(model, key), AttitudeFactor(nZ, bRef) {
182  }
183 
185  gtsam::NonlinearFactor::shared_ptr clone() const override {
186  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
187  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
188  }
189 
191  void print(const std::string& s, const KeyFormatter& keyFormatter =
192  DefaultKeyFormatter) const override;
193 
195  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
196 
198  Vector evaluateError(const Pose3& nTb, //
199  boost::optional<Matrix&> H = boost::none) const override {
200  Vector e = attitudeError(nTb.rotation(), H);
201  if (H) {
202  Matrix H23 = *H;
203  *H = Matrix::Zero(2,6);
204  H->block<2,3>(0,0) = H23;
205  }
206  return e;
207  }
208 
209 private:
210 
212  friend class boost::serialization::access;
213  template<class ARCHIVE>
214  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
215  ar & boost::serialization::make_nvp("NoiseModelFactor1",
216  boost::serialization::base_object<Base>(*this));
217  ar & boost::serialization::make_nvp("AttitudeFactor",
218  boost::serialization::base_object<AttitudeFactor>(*this));
219  }
220 
221 public:
223 };
224 
226 template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
227 
228 }
229 
gtsam::AttitudeFactor::access
friend class boost::serialization::access
Serialization function.
Definition: AttitudeFactor.h:67
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::AttitudeFactor::AttitudeFactor
AttitudeFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:51
gtsam::Rot3AttitudeFactor::evaluateError
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: AttitudeFactor.h:124
gtsam::Rot3AttitudeFactor::This
Rot3AttitudeFactor This
Typedef to this class.
Definition: AttitudeFactor.h:89
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Rot3AttitudeFactor
Definition: AttitudeFactor.h:79
gtsam::Pose3AttitudeFactor::Pose3AttitudeFactor
Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:179
gtsam::Pose3AttitudeFactor::This
Pose3AttitudeFactor This
Typedef to this class.
Definition: AttitudeFactor.h:163
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::AttitudeFactor::attitudeError
Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) const
vector of errors
Definition: AttitudeFactor.cpp:26
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::AttitudeFactor::bRef_
Unit3 bRef_
Position measurement in.
Definition: AttitudeFactor.h:38
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::Pose3AttitudeFactor::Pose3AttitudeFactor
Pose3AttitudeFactor()
default constructor - only use for serialization
Definition: AttitudeFactor.h:166
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::Pose3AttitudeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: AttitudeFactor.h:185
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Rot3AttitudeFactor::shared_ptr
boost::shared_ptr< Rot3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: AttitudeFactor.h:86
gtsam::Rot3AttitudeFactor::Rot3AttitudeFactor
Rot3AttitudeFactor()
default constructor - only use for serialization
Definition: AttitudeFactor.h:92
gtsam::Rot3
Definition: Rot3.h:59
gtsam::Pose3AttitudeFactor
Definition: AttitudeFactor.h:153
gtsam::Rot3AttitudeFactor::Rot3AttitudeFactor
Rot3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:105
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::Pose3AttitudeFactor::shared_ptr
boost::shared_ptr< Pose3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: AttitudeFactor.h:160
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Pose3
Definition: Pose3.h:37
gtsam::KeyFormatter
boost::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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::AttitudeFactor::AttitudeFactor
AttitudeFactor()
default constructor - only use for serialization
Definition: AttitudeFactor.h:43
gtsam::AttitudeFactor
Definition: AttitudeFactor.h:34
gtsam::Rot3AttitudeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: AttitudeFactor.h:111
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266
gtsam::Pose3AttitudeFactor::evaluateError
Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: AttitudeFactor.h:198