22#include <gtsam/geometry/Unit3.h>
52 nZ_(nZ),
bRef_(bRef) {
59 const Unit3& nZ()
const {
62 const Unit3& bRef()
const {
68 template<
class ARCHIVE>
69 void serialize(ARCHIVE & ar,
const unsigned int ) {
70 ar & boost::serialization::make_nvp(
"nZ_", nZ_);
71 ar & boost::serialization::make_nvp(
"bRef_",
bRef_);
86 typedef boost::shared_ptr<Rot3AttitudeFactor>
shared_ptr;
111 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
112 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
113 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
118 DefaultKeyFormatter)
const override;
125 boost::optional<Matrix&> H = boost::none)
const override {
126 return attitudeError(nRb, H);
132 friend class boost::serialization::access;
133 template<
class ARCHIVE>
134 void serialize(ARCHIVE & ar,
const unsigned int ) {
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));
186 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
187 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
188 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
193 DefaultKeyFormatter)
const override;
200 boost::optional<Matrix&> H = boost::none)
const override {
201 Vector e = attitudeError(nTb.
rotation(), H);
204 *H = Matrix::Zero(2,6);
205 H->block<2,3>(0,0) = H23;
213 friend class boost::serialization::access;
214 template<
class ARCHIVE>
215 void serialize(ARCHIVE & ar,
const unsigned int ) {
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));
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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
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