gtsam 4.1.1
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
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 NoiseModelFactor1<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 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
141public:
143};
144
146template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
147
152class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>,
153 public AttitudeFactor {
154
156
157public:
158
160 typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
161
164
167 }
168
169 ~Pose3AttitudeFactor() override {
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
209private:
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
221public:
223};
224
226template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
227
228}
229
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:277
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:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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:39
Template to create a binary predicate.
Definition: Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
Definition: Pose3.h:37
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:310
Definition: Rot3.h:59
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
This is the base class for all factor types.
Definition: Factor.h:56
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
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
Definition: AttitudeFactor.h:153
Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: AttitudeFactor.h:198
boost::shared_ptr< Pose3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: AttitudeFactor.h:160
Pose3AttitudeFactor This
Typedef to this class.
Definition: AttitudeFactor.h:163
Pose3AttitudeFactor()
default constructor - only use for serialization
Definition: AttitudeFactor.h:166
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: AttitudeFactor.h:185
Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition: AttitudeFactor.h:179
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:285