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 NoiseModelFactorN<VALUE, VALUE> {
41 
42  // Check that VALUE type is a testable Lie group
45 
46  public:
47 
48  typedef VALUE T;
49 
50  private:
51 
54 
57  public:
58 
59  // Provide access to the Matrix& version of evaluateError:
60  using Base::evaluateError;
61 
62  // shorthand for a smart pointer to a factor
63  typedef typename std::shared_ptr<BetweenFactor> shared_ptr;
64 
67 
70 
73  const SharedNoiseModel& model = nullptr) :
75  }
76 
78 
79  ~BetweenFactor() override {}
80 
83  return std::static_pointer_cast<gtsam::NonlinearFactor>(
85 
88 
90  void print(
91  const std::string& s = "",
92  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
93  std::cout << s << "BetweenFactor("
94  << keyFormatter(this->key1()) << ","
95  << keyFormatter(this->key2()) << ")\n";
96  traits<T>::Print(measured_, " measured: ");
97  this->noiseModel_->print(" noise model: ");
98  }
99 
101  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
102  const This *e = dynamic_cast<const This*> (&expected);
103  return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
104  }
105 
109 
111  Vector evaluateError(const T& p1, const T& p2,
112  OptionalMatrixType H1, OptionalMatrixType H2) const override {
113  T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
114  // manifold equivalent of h(x)-z -> log(z,h(x))
115 #ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
116  typename traits<T>::ChartJacobian::Jacobian Hlocal;
117  Vector rval = traits<T>::Local(measured_, hx, OptionalNone, (H1 || H2) ? &Hlocal : 0);
118  if (H1) *H1 = Hlocal * (*H1);
119  if (H2) *H2 = Hlocal * (*H2);
120  return rval;
121 #else
122  return traits<T>::Local(measured_, hx);
123 #endif
124  }
125 
129 
131  const VALUE& measured() const {
132  return measured_;
133  }
135 
136  private:
137 
138 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
139 
140  friend class boost::serialization::access;
141  template<class ARCHIVE>
142  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
143  // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
144  ar & boost::serialization::make_nvp("NoiseModelFactor2",
145  boost::serialization::base_object<Base>(*this));
146  ar & BOOST_SERIALIZATION_NVP(measured_);
147  }
148 #endif
149 
150  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
151  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
152  public:
154  }; // \class BetweenFactor
155 
157  template<class VALUE>
158  struct traits<BetweenFactor<VALUE> > : public Testable<BetweenFactor<VALUE> > {};
159 
165  template<class VALUE>
166  class BetweenConstraint : public BetweenFactor<VALUE> {
167  public:
168  typedef std::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
169 
171  BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
173  noiseModel::Constrained::All(traits<VALUE>::GetDimension(measured), std::abs(mu)))
174  {}
175 
176  private:
177 
179 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
180  friend class boost::serialization::access;
181  template<class ARCHIVE>
182  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
183  ar & boost::serialization::make_nvp("BetweenFactor",
184  boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
185  }
186 #endif
187  }; // \class BetweenConstraint
188 
190  template<class VALUE>
191  struct traits<BetweenConstraint<VALUE> > : public Testable<BetweenConstraint<VALUE> > {};
192 
193 }
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::BetweenConstraint::shared_ptr
std::shared_ptr< BetweenConstraint< VALUE > > shared_ptr
Definition: BetweenFactor.h:168
gtsam::BetweenFactor::This
BetweenFactor< VALUE > This
Definition: BetweenFactor.h:52
gtsam::BetweenConstraint
Definition: BetweenFactor.h:166
gtsam::BetweenFactor::BetweenFactor
BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
Definition: BetweenFactor.h:72
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::BetweenFactor::T
VALUE T
Definition: BetweenFactor.h:48
Testable.h
Concept check for values that can be used in unit tests.
gtsam::BetweenFactor::BetweenFactor
BetweenFactor()
Definition: BetweenFactor.h:69
gtsam::BetweenFactor::NeedsToAlign
@ NeedsToAlign
Definition: BetweenFactor.h:151
mu
double mu
Definition: testBoundingConstraint.cpp:37
gtsam::BetweenFactor::~BetweenFactor
~BetweenFactor() override
Definition: BetweenFactor.h:79
gtsam::NoiseModelFactorN< VALUE, VALUE >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::Factor
Definition: Factor.h:70
gtsam::BetweenFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition: BetweenFactor.h:90
gtsam::NoiseModelFactor::noiseModel_
SharedNoiseModel noiseModel_
Definition: NonlinearFactor.h:205
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::BetweenConstraint::BetweenConstraint
BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
Definition: BetweenFactor.h:171
gtsam::BetweenFactor::Base
NoiseModelFactorN< VALUE, VALUE > Base
Definition: BetweenFactor.h:53
gtsam::BetweenFactor::GTSAM_CONCEPT_ASSERT
GTSAM_CONCEPT_ASSERT(IsTestable< VALUE >)
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
OptionalNone
#define OptionalNone
Definition: NonlinearFactor.h:49
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
gtsam::KeyFormatter
std::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::BetweenFactor::shared_ptr
std::shared_ptr< BetweenFactor > shared_ptr
Definition: BetweenFactor.h:63
gtsam::IsLieGroup
Definition: Lie.h:260
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::BetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition: BetweenFactor.h:101
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::NoiseModelFactorN< VALUE, VALUE >::key1
Key key1() const
Definition: NonlinearFactor.h:731
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< VALUE, VALUE >::key2
Key key2() const
Definition: NonlinearFactor.h:735
Between
BetweenFactor< Rot3 > Between
Definition: testRot3Optimization.cpp:31
Lie.h
Base class and basic functions for Lie types.
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::BetweenFactor::evaluateError
Vector evaluateError(const T &p1, const T &p2, OptionalMatrixType H1, OptionalMatrixType H2) const override
evaluate error, returns vector of errors size of tangent space
Definition: BetweenFactor.h:111
gtsam::traits
Definition: Group.h:36
gtsam::BetweenFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: BetweenFactor.h:82
std
Definition: BFloat16.h:88
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
abs
#define abs(x)
Definition: datatypes.h:17
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
Definition: types.h:288
gtsam::IsTestable
Definition: Testable.h:59
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::BetweenFactor::measured_
VALUE measured_
Definition: BetweenFactor.h:55
gtsam::BetweenFactor::measured
const VALUE & measured() const
return the measurement
Definition: BetweenFactor.h:131
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:01