gtsam 4.2.0
gtsam
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
22#include <gtsam/geometry/Unit3.h>
23
24namespace gtsam {
25
35
36protected:
37
38 Unit3 nZ_, bRef_;
39
40public:
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
79class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {
80
82
83public:
84
86 typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
87
90
93 }
94
95 ~Rot3AttitudeFactor() override {
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
129private:
130
132 friend class boost::serialization::access;
133 template<class ARCHIVE>
134 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
135 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
136 ar & boost::serialization::make_nvp("NoiseModelFactor1",
137 boost::serialization::base_object<Base>(*this));
138 ar & boost::serialization::make_nvp("AttitudeFactor",
139 boost::serialization::base_object<AttitudeFactor>(*this));
140 }
141
142public:
144};
145
147template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
148
153class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
154 public AttitudeFactor {
155
157
158public:
159
161 typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
162
165
168 }
169
170 ~Pose3AttitudeFactor() override {
171 }
172
180 Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
181 const Unit3& bRef = Unit3(0, 0, 1)) :
182 Base(model, key), AttitudeFactor(nZ, bRef) {
183 }
184
186 gtsam::NonlinearFactor::shared_ptr clone() const override {
187 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
188 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
189 }
190
192 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
193 DefaultKeyFormatter) const override;
194
196 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
197
199 Vector evaluateError(const Pose3& nTb, //
200 boost::optional<Matrix&> H = boost::none) const override {
201 Vector e = attitudeError(nTb.rotation(), H);
202 if (H) {
203 Matrix H23 = *H;
204 *H = Matrix::Zero(2,6);
205 H->block<2,3>(0,0) = H23;
206 }
207 return e;
208 }
209
210private:
211
213 friend class boost::serialization::access;
214 template<class ARCHIVE>
215 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
216 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
217 ar & boost::serialization::make_nvp("NoiseModelFactor1",
218 boost::serialization::base_object<Base>(*this));
219 ar & boost::serialization::make_nvp("AttitudeFactor",
220 boost::serialization::base_object<AttitudeFactor>(*this));
221 }
222
223public:
225};
226
228template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
229
230}
231
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:308
3D Pose
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
Template to create a binary predicate.
Definition: Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:315
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Represents a 3D point on a unit sphere.
Definition: Unit3.h:43
Definition: Factor.h:68
Base class for prior on attitude Example:
Definition: AttitudeFactor.h:34
Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) const
vector of errors
Definition: AttitudeFactor.cpp:26
Unit3 bRef_
Position measurement in.
Definition: AttitudeFactor.h:38
AttitudeFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:51
friend class boost::serialization::access
Serialization function.
Definition: AttitudeFactor.h:67
AttitudeFactor()
default constructor - only use for serialization
Definition: AttitudeFactor.h:43
Version of AttitudeFactor for Rot3.
Definition: AttitudeFactor.h:79
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: AttitudeFactor.h:124
Rot3AttitudeFactor()
default constructor - only use for serialization
Definition: AttitudeFactor.h:92
boost::shared_ptr< Rot3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: AttitudeFactor.h:86
Rot3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:105
Rot3AttitudeFactor This
Typedef to this class.
Definition: AttitudeFactor.h:89
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: AttitudeFactor.h:111
Version of AttitudeFactor for Pose3.
Definition: AttitudeFactor.h:154
Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: AttitudeFactor.h:199
boost::shared_ptr< Pose3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: AttitudeFactor.h:161
Pose3AttitudeFactor This
Typedef to this class.
Definition: AttitudeFactor.h:164
Pose3AttitudeFactor()
default constructor - only use for serialization
Definition: AttitudeFactor.h:167
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: AttitudeFactor.h:186
Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:180
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400