gtsam  4.1.0
gtsam
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 
21 #include <gtsam/nonlinear/ExpressionFactor.h>
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:
37  typedef RangeFactor<A1, A2> This;
39 
40  public:
43 
44  RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model)
45  : Base({key1, key2}, model, measured) {
46  this->initialize(expression({key1, key2}));
47  }
48 
50  gtsam::NonlinearFactor::shared_ptr clone() const override {
51  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
52  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
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,
63  boost::optional<Matrix&> H1 = boost::none,
64  boost::optional<Matrix&> H2 = boost::none) const
65  {
66  std::vector<Matrix> Hs(2);
67  const auto &keys = Factor::keys();
68  const Vector error = Base::unwhitenedError(
69  {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
70  Hs);
71  if (H1) *H1 = Hs[0];
72  if (H2) *H2 = Hs[1];
73  return error;
74  }
75 
77  void print(const std::string& s = "",
78  const KeyFormatter& kf = DefaultKeyFormatter) const override {
79  std::cout << s << "RangeFactor" << std::endl;
80  Base::print(s, kf);
81  }
82 
83  private:
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 }; // \ RangeFactor
91 
93 template <typename A1, typename A2, typename T>
94 struct traits<RangeFactor<A1, A2, T> >
95  : public Testable<RangeFactor<A1, A2, T> > {};
96 
101 template <typename A1, typename A2 = A1,
102  typename T = typename Range<A1, A2>::result_type>
103 class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
104  private:
107 
108  A1 body_T_sensor_;
109 
110  public:
113 
115  const SharedNoiseModel& model,
116  const A1& body_T_sensor)
117  : Base({key1, key2}, model, measured), body_T_sensor_(body_T_sensor) {
118  this->initialize(expression({key1, key2}));
119  }
120 
121  virtual ~RangeFactorWithTransform() {}
122 
124  gtsam::NonlinearFactor::shared_ptr clone() const override {
125  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
126  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
127  }
128 
129  // Return measurement expression
130  Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
131  Expression<A1> body_T_sensor__(body_T_sensor_);
132  Expression<A1> nav_T_body_(keys[0]);
133  Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
134  body_T_sensor__);
135  Expression<A2> a2_(keys[1]);
136  return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
137  }
138 
139  Vector evaluateError(const A1& a1, const A2& a2,
140  boost::optional<Matrix&> H1 = boost::none,
141  boost::optional<Matrix&> H2 = boost::none) const
142  {
143  std::vector<Matrix> Hs(2);
144  const auto &keys = Factor::keys();
145  const Vector error = Base::unwhitenedError(
146  {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
147  Hs);
148  if (H1) *H1 = Hs[0];
149  if (H2) *H2 = Hs[1];
150  return error;
151  }
152 
154  void print(const std::string& s = "",
155  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
156  std::cout << s << "RangeFactorWithTransform" << std::endl;
157  this->body_T_sensor_.print(" sensor pose in body frame: ");
158  Base::print(s, keyFormatter);
159  }
160 
161  private:
164  template <typename ARCHIVE>
165  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
166  // **IMPORTANT** We need to (de)serialize parameters before the base class,
167  // since it calls expression() and we need all parameters ready at that
168  // point.
169  ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
170  ar& boost::serialization::make_nvp(
171  "Base", boost::serialization::base_object<Base>(*this));
172  }
173 }; // \ RangeFactorWithTransform
174 
176 template <typename A1, typename A2, typename T>
177 struct traits<RangeFactorWithTransform<A1, A2, T> >
178  : public Testable<RangeFactorWithTransform<A1, A2, T> > {};
179 
180 } // \ namespace gtsam
gtsam::RangeFactorWithTransform::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition: RangeFactor.h:154
gtsam::RangeFactor::print
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Definition: RangeFactor.h:77
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition: ExpressionFactor.h:99
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::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ExpressionFactor< double >::measured
const double & measured() const
return the measurement
Definition: ExpressionFactor.h:77
gtsam::RangeFactor::RangeFactor
RangeFactor()
default constructor
Definition: RangeFactor.h:42
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition: GenericValue.h:212
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::RangeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RangeFactor.h:50
gtsam::RangeFactor
Definition: RangeFactor.h:35
gtsam::RangeFactorWithTransform::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RangeFactor.h:124
gtsam::Range
Definition: BearingRange.h:39
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::Expression
Expression class that supports automatic differentiation.
Definition: Expression.h:49
gtsam::RangeFactorWithTransform
Definition: RangeFactor.h:103
gtsam::ExpressionFactorN
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition: ExpressionFactor.h:242
gtsam::ExpressionFactor< double >::initialize
void initialize(const Expression< double > &expression)
Initialize with constructor arguments.
Definition: ExpressionFactor.h:167
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:80
gtsam::RangeFactorWithTransform::access
friend class boost::serialization::access
Serialization function.
Definition: RangeFactor.h:163