gtsam  4.1.0
gtsam
BetweenFactor.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 
18 #include <ostream>
19 
20 #include <gtsam/base/Testable.h>
21 #include <gtsam/base/Lie.h>
23 
24 #ifdef _WIN32
25 #define BETWEENFACTOR_VISIBILITY
26 #else
27 // This will trigger a LNKxxxx on MSVC, so disable for MSVC build
28 // Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
29 #define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT
30 #endif
31 
32 namespace gtsam {
33 
39  template<class VALUE>
40  class BetweenFactor: public NoiseModelFactor2<VALUE, VALUE> {
41 
42  // Check that VALUE type is a testable Lie group
43  BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
44  BOOST_CONCEPT_ASSERT((IsLieGroup<VALUE>));
45 
46  public:
47 
48  typedef VALUE T;
49 
50  private:
51 
52  typedef BetweenFactor<VALUE> This;
54 
55  VALUE measured_;
57  public:
58 
59  // shorthand for a smart pointer to a factor
60  typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
61 
64 
66  BetweenFactor(Key key1, Key key2, const VALUE& measured,
67  const SharedNoiseModel& model = nullptr) :
68  Base(model, key1, key2), measured_(measured) {
69  }
70 
71  virtual ~BetweenFactor() {}
72 
74  gtsam::NonlinearFactor::shared_ptr clone() const override {
75  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
76  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
77 
81 
83  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
84  std::cout << s << "BetweenFactor("
85  << keyFormatter(this->key1()) << ","
86  << keyFormatter(this->key2()) << ")\n";
87  traits<T>::Print(measured_, " measured: ");
88  this->noiseModel_->print(" noise model: ");
89  }
90 
92  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
93  const This *e = dynamic_cast<const This*> (&expected);
94  return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
95  }
96 
100 
102  Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
103  boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
104  T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
105  // manifold equivalent of h(x)-z -> log(z,h(x))
106 #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR
107  typename traits<T>::ChartJacobian::Jacobian Hlocal;
108  Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
109  if (H1) *H1 = Hlocal * (*H1);
110  if (H2) *H2 = Hlocal * (*H2);
111  return rval;
112 #else
113  return traits<T>::Local(measured_, hx);
114 #endif
115  }
116 
120 
122  const VALUE& measured() const {
123  return measured_;
124  }
126 
127  private:
128 
131  template<class ARCHIVE>
132  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
133  ar & boost::serialization::make_nvp("NoiseModelFactor2",
134  boost::serialization::base_object<Base>(*this));
135  ar & BOOST_SERIALIZATION_NVP(measured_);
136  }
137 
138  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
139  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
140  public:
142  }; // \class BetweenFactor
143 
145  template<class VALUE>
146  struct traits<BetweenFactor<VALUE> > : public Testable<BetweenFactor<VALUE> > {};
147 
153  template<class VALUE>
154  class BetweenConstraint : public BetweenFactor<VALUE> {
155  public:
156  typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
157 
159  BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
160  BetweenFactor<VALUE>(key1, key2, measured,
161  noiseModel::Constrained::All(traits<VALUE>::GetDimension(measured), std::abs(mu)))
162  {}
163 
164  private:
165 
168  template<class ARCHIVE>
169  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
170  ar & boost::serialization::make_nvp("BetweenFactor",
171  boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
172  }
173  }; // \class BetweenConstraint
174 
176  template<class VALUE>
177  struct traits<BetweenConstraint<VALUE> > : public Testable<BetweenConstraint<VALUE> > {};
178 
179 }
Testable.h
Concept check for values that can be used in unit tests.
gtsam::IsLieGroup
Lie Group Concept.
Definition: Lie.h:259
gtsam::BetweenConstraint::access
friend class boost::serialization::access
Serialization function.
Definition: BetweenFactor.h:167
gtsam::BetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition: BetweenFactor.h:92
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
gtsam::BetweenFactor::access
friend class boost::serialization::access
Serialization function.
Definition: BetweenFactor.h:130
gtsam::BetweenFactor::measured
const VALUE & measured() const
return the measurement
Definition: BetweenFactor.h:122
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
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::IsTestable
Definition: Testable.h:57
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::BetweenConstraint
Binary between constraint - forces between to a given value This constraint requires the underlying t...
Definition: BetweenFactor.h:154
gtsam::NoiseModelFactor2< VALUE, VALUE >::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::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::BetweenFactor::BetweenFactor
BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
Constructor.
Definition: BetweenFactor.h:66
gtsam::BetweenFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition: BetweenFactor.h:83
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::BetweenConstraint::BetweenConstraint
BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
Syntactic sugar for constrained version.
Definition: BetweenFactor.h:159
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::BetweenFactor::evaluateError
Vector evaluateError(const T &p1, const T &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
evaluate error, returns vector of errors size of tangent space
Definition: BetweenFactor.h:102
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition: types.h:278
gtsam::BetweenFactor::BetweenFactor
BetweenFactor()
default constructor - only use for serialization
Definition: BetweenFactor.h:63
gtsam::BetweenFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: BetweenFactor.h:74
Lie.h
Base class and basic functions for Lie types.
gtsam::BetweenFactor::shared_ptr
boost::shared_ptr< BetweenFactor > shared_ptr
The measurement.
Definition: BetweenFactor.h:60