sam/RangeFactor.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 
19 #pragma once
20 
22 
23 namespace gtsam {
24 
25 // forward declaration of Range functor, assumed partially specified
26 template <typename A1, typename A2>
27 struct Range;
28 
34 template <typename A1, typename A2 = A1, typename T = double>
35 class RangeFactor : public ExpressionFactorN<T, A1, A2> {
36  private:
39 
40  public:
43 
45  : Base({key1, key2}, model, measured) {
46  this->initialize(expression({key1, key2}));
47  }
48 
51  return std::static_pointer_cast<gtsam::NonlinearFactor>(
53  }
54 
55  // Return measurement expression
56  Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
57  Expression<A1> a1_(keys[0]);
58  Expression<A2> a2_(keys[1]);
59  return Expression<T>(Range<A1, A2>(), a1_, a2_);
60  }
61 
62  Vector evaluateError(const A1& a1, const A2& a2,
64  OptionalMatrixType H2 = OptionalNone) const {
65  std::vector<Matrix> Hs(2);
66  const auto& keys = Factor::keys();
68  {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
69  Hs);
70  if (H1) *H1 = Hs[0];
71  if (H2) *H2 = Hs[1];
72  return error;
73  }
74 
76  void print(const std::string& s = "",
77  const KeyFormatter& kf = DefaultKeyFormatter) const override {
78  std::cout << s << "RangeFactor" << std::endl;
79  Base::print(s, kf);
80  }
81 
82  private:
83 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
84  friend class boost::serialization::access;
85  template <class ARCHIVE>
86  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
87  ar& boost::serialization::make_nvp(
88  "Base", boost::serialization::base_object<Base>(*this));
89  }
90 #endif
91 }; // \ RangeFactor
92 
94 template <typename A1, typename A2, typename T>
95 struct traits<RangeFactor<A1, A2, T> >
96  : public Testable<RangeFactor<A1, A2, T> > {};
97 
102 template <typename A1, typename A2 = A1,
103  typename T = typename Range<A1, A2>::result_type>
104 class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
105  private:
108 
110 
111  public:
114 
116  const SharedNoiseModel& model,
117  const A1& body_T_sensor)
118  : Base({key1, key2}, model, measured), body_T_sensor_(body_T_sensor) {
119  this->initialize(expression({key1, key2}));
120  }
121 
123 
126  return std::static_pointer_cast<gtsam::NonlinearFactor>(
128  }
129 
130  // Return measurement expression
131  Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
132  Expression<A1> body_T_sensor__(body_T_sensor_);
133  Expression<A1> nav_T_body_(keys[0]);
134  Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
135  body_T_sensor__);
136  Expression<A2> a2_(keys[1]);
137  return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
138  }
139 
140  Vector evaluateError(const A1& a1, const A2& a2,
142  std::vector<Matrix> Hs(2);
143  const auto &keys = Factor::keys();
145  {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
146  Hs);
147  if (H1) *H1 = Hs[0];
148  if (H2) *H2 = Hs[1];
149  return error;
150  }
151 
152  // An evaluateError overload to accept matrices (Matrix&) and pass it to the
153  // OptionalMatrixType evaluateError overload
154  Vector evaluateError(const A1& a1, const A2& a2, Matrix& H1, Matrix& H2) const {
155  return evaluateError(a1, a2, &H1, &H2);
156  }
157 
159  void print(const std::string& s = "",
160  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
161  std::cout << s << "RangeFactorWithTransform" << std::endl;
162  this->body_T_sensor_.print(" sensor pose in body frame: ");
163  Base::print(s, keyFormatter);
164  }
165 
166  private:
167 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
168  friend class boost::serialization::access;
170  template <typename ARCHIVE>
171  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
172  // **IMPORTANT** We need to (de)serialize parameters before the base class,
173  // since it calls expression() and we need all parameters ready at that
174  // point.
175  ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
176  ar& boost::serialization::make_nvp(
177  "Base", boost::serialization::base_object<Base>(*this));
178  }
179 #endif
180 }; // \ RangeFactorWithTransform
181 
183 template <typename A1, typename A2, typename T>
185  : public Testable<RangeFactorWithTransform<A1, A2, T> > {};
186 
187 } // \ namespace gtsam
key1
const Symbol key1('v', 1)
gtsam::RangeFactor::evaluateError
Vector evaluateError(const A1 &a1, const A2 &a2, OptionalMatrixType H1=OptionalNone, OptionalMatrixType H2=OptionalNone) const
Definition: sam/RangeFactor.h:62
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::RangeFactorWithTransform::This
RangeFactorWithTransform< A1, A2 > This
Definition: sam/RangeFactor.h:106
gtsam::RangeFactor
Definition: sam/RangeFactor.h:35
gtsam::RangeFactor::This
RangeFactor< A1, A2 > This
Definition: sam/RangeFactor.h:37
gtsam::RangeFactor::expression
Expression< T > expression(const typename Base::ArrayNKeys &keys) const override
Definition: sam/RangeFactor.h:56
s
RealScalar s
Definition: level1_cplx_impl.h:126
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::RangeFactorWithTransform::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: sam/RangeFactor.h:159
gtsam::RangeFactorWithTransform::expression
Expression< T > expression(const typename Base::ArrayNKeys &keys) const override
Definition: sam/RangeFactor.h:131
gtsam::RangeFactorWithTransform::evaluateError
Vector evaluateError(const A1 &a1, const A2 &a2, Matrix &H1, Matrix &H2) const
Definition: sam/RangeFactor.h:154
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:210
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::Expression
Definition: Expression.h:47
gtsam::RangeFactor::RangeFactor
RangeFactor()
default constructor
Definition: sam/RangeFactor.h:42
align_3::a1
Point2 a1
Definition: testPose2.cpp:769
key2
const Symbol key2('v', 2)
gtsam::RangeFactor::RangeFactor
RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel &model)
Definition: sam/RangeFactor.h:44
OptionalNone
#define OptionalNone
Definition: NonlinearFactor.h:50
gtsam::RangeFactorWithTransform::~RangeFactorWithTransform
~RangeFactorWithTransform() override
Definition: sam/RangeFactor.h:122
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::ExpressionFactor< double >::initialize
void initialize(const Expression< double > &expression)
Initialize with constructor arguments.
Definition: ExpressionFactor.h:172
gtsam::RangeFactorWithTransform::evaluateError
Vector evaluateError(const A1 &a1, const A2 &a2, OptionalMatrixType H1=OptionalNone, OptionalMatrixType H2=OptionalNone) const
Definition: sam/RangeFactor.h:140
A2
static const double A2[]
Definition: expn.h:7
gtsam::RangeFactorWithTransform::RangeFactorWithTransform
RangeFactorWithTransform(Key key1, Key key2, T measured, const SharedNoiseModel &model, const A1 &body_T_sensor)
Definition: sam/RangeFactor.h:115
gtsam::ExpressionFactorN< double, A1, A1 >::expression
Expression< double > expression() const override
Return an expression that predicts the measurement given Values.
Definition: ExpressionFactor.h:282
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Eigen::Triplet< double >
gtsam::ExpressionFactorN::ArrayNKeys
std::array< Key, NARY_EXPRESSION_SIZE > ArrayNKeys
Definition: ExpressionFactor.h:252
gtsam::Range
Definition: BearingRange.h:41
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::ExpressionFactor< double >::measured
const double & measured() const
Definition: ExpressionFactor.h:82
Range
pair< size_t, size_t > Range
Definition: testBTree.cpp:27
gtsam::RangeFactorWithTransform::RangeFactorWithTransform
RangeFactorWithTransform()
Definition: sam/RangeFactor.h:113
gtsam::RangeFactorWithTransform
Definition: sam/RangeFactor.h:104
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Definition: ExpressionFactor.h:104
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
align_3::a2
Point2 a2
Definition: testPose2.cpp:770
gtsam::RangeFactor::print
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Definition: sam/RangeFactor.h:76
A1
static const double A1[]
Definition: expn.h:6
gtsam::ExpressionFactorN
Definition: ExpressionFactor.h:249
gtsam::RangeFactorWithTransform::body_T_sensor_
A1 body_T_sensor_
The pose of the sensor in the body frame.
Definition: sam/RangeFactor.h:109
gtsam::RangeFactorWithTransform::Base
ExpressionFactorN< T, A1, A2 > Base
Definition: sam/RangeFactor.h:107
ExpressionFactor.h
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
gtsam::RangeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: sam/RangeFactor.h:50
gtsam::RangeFactorWithTransform::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: sam/RangeFactor.h:125
gtsam::ExpressionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print relies on Testable traits being defined for T
Definition: ExpressionFactor.h:85
gtsam::RangeFactor::Base
ExpressionFactorN< T, A1, A2 > Base
Definition: sam/RangeFactor.h:38
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:03