gtsam  4.1.0
gtsam
PosePriorFactor.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 
16 #pragma once
17 
20 #include <gtsam/base/Testable.h>
21 
22 namespace gtsam {
23 
28  template<class POSE>
29  class PosePriorFactor: public NoiseModelFactor1<POSE> {
30 
31  private:
32 
35 
36  POSE prior_;
37  boost::optional<POSE> body_P_sensor_;
38 
40  GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
41  GTSAM_CONCEPT_POSE_TYPE(POSE)
42  public:
43 
45  typedef typename boost::shared_ptr<PosePriorFactor<POSE> > shared_ptr;
46 
49 
50  virtual ~PosePriorFactor() {}
51 
53  PosePriorFactor(Key key, const POSE& prior, const SharedNoiseModel& model,
54  boost::optional<POSE> body_P_sensor = boost::none) :
55  Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) {
56  }
57 
59  gtsam::NonlinearFactor::shared_ptr clone() const override {
60  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
61  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
62 
66  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
67  std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
68  prior_.print(" prior mean: ");
69  if(this->body_P_sensor_)
70  this->body_P_sensor_->print(" sensor pose in body frame: ");
71  this->noiseModel_->print(" noise model: ");
72  }
73 
75  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
76  const This* e = dynamic_cast<const This*> (&expected);
77  return e != nullptr
78  && Base::equals(*e, tol)
79  && this->prior_.equals(e->prior_, tol)
80  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
81  }
82 
86  Vector evaluateError(const POSE& p, boost::optional<Matrix&> H = boost::none) const override {
87  if(body_P_sensor_) {
88  // manifold equivalent of h(x)-z -> log(z,h(x))
89  return prior_.localCoordinates(p.compose(*body_P_sensor_, H));
90  } else {
91  if(H) (*H) = I_6x6;
92  // manifold equivalent of h(x)-z -> log(z,h(x))
93  return prior_.localCoordinates(p);
94  }
95  }
96 
97  const POSE& prior() const { return prior_; }
98 
99  private:
100 
103  template<class ARCHIVE>
104  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
105  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
106  ar & BOOST_SERIALIZATION_NVP(prior_);
107  ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
108  }
109  };
110 
111 }
Testable.h
Concept check for values that can be used in unit tests.
gtsam::PosePriorFactor::evaluateError
Vector evaluateError(const POSE &p, boost::optional< Matrix & > H=boost::none) const override
implement functions needed to derive from Factor
Definition: PosePriorFactor.h:86
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::PosePriorFactor::PosePriorFactor
PosePriorFactor(Key key, const POSE &prior, const SharedNoiseModel &model, boost::optional< POSE > body_P_sensor=boost::none)
Constructor.
Definition: PosePriorFactor.h:53
gtsam::PosePriorFactor::shared_ptr
boost::shared_ptr< PosePriorFactor< POSE > > shared_ptr
concept check by type
Definition: PosePriorFactor.h:45
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::PosePriorFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: PosePriorFactor.h:59
gtsam::PosePriorFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: PosePriorFactor.h:66
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
gtsam::PosePriorFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: PosePriorFactor.h:75
NonlinearFactor.h
Non-linear factor base classes.
gtsam::PosePriorFactor::access
friend class boost::serialization::access
Serialization function.
Definition: PosePriorFactor.h:102
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::PosePriorFactor
Definition: PosePriorFactor.h:29
gtsam::PosePriorFactor::PosePriorFactor
PosePriorFactor()
default constructor - only use for serialization
Definition: PosePriorFactor.h:48
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42