gtsam  4.1.0
gtsam
PoseRotationPrior.h
Go to the documentation of this file.
1 
10 #pragma once
11 
14 
15 
16 namespace gtsam {
17 
18 template<class POSE>
19 class PoseRotationPrior : public NoiseModelFactor1<POSE> {
20 public:
21 
24  typedef POSE Pose;
25  typedef typename POSE::Translation Translation;
26  typedef typename POSE::Rotation Rotation;
27 
28  GTSAM_CONCEPT_POSE_TYPE(Pose)
29  GTSAM_CONCEPT_GROUP_TYPE(Pose)
30  GTSAM_CONCEPT_LIE_TYPE(Rotation)
31 
32  // Get dimensions of pose and rotation type at compile time
33  static const int xDim = FixedDimension<POSE>::value;
34  static const int rDim = FixedDimension<typename POSE::Rotation>::value;
35 
36 protected:
37 
38  Rotation measured_;
39 
40 public:
41 
43  PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
44  : Base(model, key), measured_(rot_z) {}
45 
47  PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
48  : Base(model, key), measured_(pose_z.rotation()) {}
49 
50  virtual ~PoseRotationPrior() {}
51 
53  gtsam::NonlinearFactor::shared_ptr clone() const override {
54  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
55  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
56 
57  // access
58  const Rotation& measured() const { return measured_; }
59 
60  // testable
61 
63  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
64  const This *e = dynamic_cast<const This*> (&expected);
65  return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
66  }
67 
69  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
70  Base::print(s + "PoseRotationPrior", keyFormatter);
71  measured_.print("Measured Rotation");
72  }
73 
75  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
76  const Rotation& newR = pose.rotation();
77  if (H) {
78  *H = Matrix::Zero(rDim, xDim);
79  std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
80  (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
81  }
82 
83  return measured_.localCoordinates(newR);
84  }
85 
86 private:
87 
90  template<class ARCHIVE>
91  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
92  ar & boost::serialization::make_nvp("NoiseModelFactor1",
93  boost::serialization::base_object<Base>(*this));
94  ar & BOOST_SERIALIZATION_NVP(measured_);
95  }
96 };
97 
98 } // \namespace gtsam
99 
100 
101 
102 
gtsam::PoseRotationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
h(x)-z
Definition: PoseRotationPrior.h:75
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::PoseRotationPrior
Definition: PoseRotationPrior.h:19
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::PoseRotationPrior::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition: PoseRotationPrior.h:69
gtsam::PoseRotationPrior::access
friend class boost::serialization::access
Serialization function.
Definition: PoseRotationPrior.h:89
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
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::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel &model)
standard constructor
Definition: PoseRotationPrior.h:43
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model)
Constructor that pulls the translation from an incoming POSE.
Definition: PoseRotationPrior.h:47
gtsam::PoseRotationPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals specialized to this factor
Definition: PoseRotationPrior.h:63
gtsam::PoseRotationPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: PoseRotationPrior.h:53