gtsam  4.1.0
gtsam
PoseBetweenFactor.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 
16 #pragma once
17 
18 #include <ostream>
19 
22 #include <gtsam/base/Testable.h>
23 
24 namespace gtsam {
25 
31  template<class POSE>
32  class PoseBetweenFactor: public NoiseModelFactor2<POSE, POSE> {
33 
34  private:
35 
38 
39  POSE measured_;
40  boost::optional<POSE> body_P_sensor_;
41 
43  GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
44  GTSAM_CONCEPT_POSE_TYPE(POSE)
45  public:
46 
47  // shorthand for a smart pointer to a factor
48  typedef typename boost::shared_ptr<PoseBetweenFactor> shared_ptr;
49 
52 
54  PoseBetweenFactor(Key key1, Key key2, const POSE& measured,
55  const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
56  Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) {
57  }
58 
59  virtual ~PoseBetweenFactor() {}
60 
62  gtsam::NonlinearFactor::shared_ptr clone() const override {
63  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
64  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
65 
69  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
70  std::cout << s << "BetweenFactor("
71  << keyFormatter(this->key1()) << ","
72  << keyFormatter(this->key2()) << ")\n";
73  measured_.print(" measured: ");
74  if(this->body_P_sensor_)
75  this->body_P_sensor_->print(" sensor pose in body frame: ");
76  this->noiseModel_->print(" noise model: ");
77  }
78 
80  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
81  const This *e = dynamic_cast<const This*> (&expected);
82  return e != nullptr
83  && Base::equals(*e, tol)
84  && this->measured_.equals(e->measured_, tol)
85  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
86  }
87 
91  Vector evaluateError(const POSE& p1, const POSE& p2,
92  boost::optional<Matrix&> H1 = boost::none,
93  boost::optional<Matrix&> H2 = boost::none) const override {
94  if(body_P_sensor_) {
95  POSE hx;
96  if(H1 || H2) {
97  Matrix H3;
98  hx = p1.compose(*body_P_sensor_,H3).between(p2.compose(*body_P_sensor_), H1, H2); // h(x)
99  if(H1) (*H1) *= H3;
100  if(H2) (*H2) *= H3;
101  } else {
102  hx = p1.compose(*body_P_sensor_).between(p2.compose(*body_P_sensor_)); // h(x)
103  }
104  // manifold equivalent of h(x)-z -> log(z,h(x))
105  return measured_.localCoordinates(hx);
106  } else {
107  POSE hx = p1.between(p2, H1, H2); // h(x)
108  // manifold equivalent of h(x)-z -> log(z,h(x))
109  return measured_.localCoordinates(hx);
110  }
111  }
112 
114  const POSE& measured() const {
115  return measured_;
116  }
117 
118  private:
119 
122  template<class ARCHIVE>
123  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
124  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
125  ar & BOOST_SERIALIZATION_NVP(measured_);
126 
127  }
128  }; // \class PoseBetweenFactor
129 
130 }
Testable.h
Concept check for values that can be used in unit tests.
gtsam::PoseBetweenFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: PoseBetweenFactor.h:62
gtsam::PoseBetweenFactor::shared_ptr
boost::shared_ptr< PoseBetweenFactor > shared_ptr
concept check by type
Definition: PoseBetweenFactor.h:48
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::PoseBetweenFactor::access
friend class boost::serialization::access
Serialization function.
Definition: PoseBetweenFactor.h:121
gtsam::NoiseModelFactor2< POSE, POSE >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
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::PoseBetweenFactor::measured
const POSE & measured() const
return the measured
Definition: PoseBetweenFactor.h:114
gtsam::PoseBetweenFactor::evaluateError
Vector evaluateError(const POSE &p1, const POSE &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
implement functions needed to derive from Factor
Definition: PoseBetweenFactor.h:91
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::PoseBetweenFactor::PoseBetweenFactor
PoseBetweenFactor(Key key1, Key key2, const POSE &measured, const SharedNoiseModel &model, boost::optional< POSE > body_P_sensor=boost::none)
Constructor.
Definition: PoseBetweenFactor.h:54
gtsam::PoseBetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: PoseBetweenFactor.h:80
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::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::PoseBetweenFactor
Definition: PoseBetweenFactor.h:32
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::PoseBetweenFactor::PoseBetweenFactor
PoseBetweenFactor()
default constructor - only use for serialization
Definition: PoseBetweenFactor.h:51
gtsam::PoseBetweenFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: PoseBetweenFactor.h:69