gtsam  4.1.0
gtsam
PoseTranslationPrior.h
Go to the documentation of this file.
1 
10 #pragma once
11 
14 
15 namespace gtsam {
16 
20 template<class POSE>
22 public:
25  typedef POSE Pose;
26  typedef typename POSE::Translation Translation;
27  typedef typename POSE::Rotation Rotation;
28 
29  GTSAM_CONCEPT_POSE_TYPE(Pose)
30  GTSAM_CONCEPT_GROUP_TYPE(Pose)
31  GTSAM_CONCEPT_LIE_TYPE(Translation)
32 
33 protected:
34 
35  Translation measured_;
36 
37 public:
38 
41 
43  PoseTranslationPrior(Key key, const Translation& measured, const noiseModel::Base::shared_ptr& model)
44  : Base(model, key), measured_(measured) {
45  }
46 
48  PoseTranslationPrior(Key key, const POSE& pose_z, const noiseModel::Base::shared_ptr& model)
49  : Base(model, key), measured_(pose_z.translation()) {
50  }
51 
52  virtual ~PoseTranslationPrior() {}
53 
54  const Translation& measured() const { return measured_; }
55 
57  gtsam::NonlinearFactor::shared_ptr clone() const override {
58  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
59  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
60 
62  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
63  const Translation& newTrans = pose.translation();
64  const Rotation& R = pose.rotation();
65  const int tDim = traits<Translation>::GetDimension(newTrans);
66  const int xDim = traits<Pose>::GetDimension(pose);
67  if (H) {
68  *H = Matrix::Zero(tDim, xDim);
69  std::pair<size_t, size_t> transInterval = POSE::translationInterval();
70  (*H).middleCols(transInterval.first, tDim) = R.matrix();
71  }
72 
73  return traits<Translation>::Local(measured_, newTrans);
74  }
75 
77  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
78  const This *e = dynamic_cast<const This*> (&expected);
79  return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
80  }
81 
83  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
84  Base::print(s + "PoseTranslationPrior", keyFormatter);
85  traits<Translation>::Print(measured_, "Measured Translation");
86  }
87 
88 private:
89 
92  template<class ARCHIVE>
93  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
94  ar & boost::serialization::make_nvp("NoiseModelFactor1",
95  boost::serialization::base_object<Base>(*this));
96  ar & BOOST_SERIALIZATION_NVP(measured_);
97  }
98 
99 };
100 
101 } // \namespace gtsam
102 
103 
104 
105 
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
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::PoseTranslationPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals specialized to this factor
Definition: PoseTranslationPrior.h:77
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior()
default constructor - only use for serialization
Definition: PoseTranslationPrior.h:40
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
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::PoseTranslationPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: PoseTranslationPrior.h:57
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior(Key key, const Translation &measured, const noiseModel::Base::shared_ptr &model)
standard constructor
Definition: PoseTranslationPrior.h:43
gtsam::PoseTranslationPrior::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition: PoseTranslationPrior.h:83
gtsam::PoseTranslationPrior
A prior on the translation part of a pose.
Definition: PoseTranslationPrior.h:21
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::PoseTranslationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
h(x)-z
Definition: PoseTranslationPrior.h:62
gtsam::PoseTranslationPrior::access
friend class boost::serialization::access
Serialization function.
Definition: PoseTranslationPrior.h:91
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior(Key key, const POSE &pose_z, const noiseModel::Base::shared_ptr &model)
Constructor that pulls the translation from an incoming POSE.
Definition: PoseTranslationPrior.h:48