gtsam  4.1.0
gtsam
BearingRangeFactor.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 
20 #pragma once
21 
22 #include <gtsam/nonlinear/ExpressionFactor.h>
24 #include <boost/concept/assert.hpp>
25 
26 namespace gtsam {
27 
32 template <typename A1, typename A2,
33  typename B = typename Bearing<A1, A2>::result_type,
34  typename R = typename Range<A1, A2>::result_type>
36  : public ExpressionFactorN<BearingRange<A1, A2>, A1, A2> {
37  private:
38  typedef BearingRange<A1, A2> T;
41 
42  public:
43  typedef boost::shared_ptr<This> shared_ptr;
44 
47 
49  BearingRangeFactor(Key key1, Key key2, const T &bearingRange,
50  const SharedNoiseModel &model)
51  : Base({{key1, key2}}, model, T(bearingRange)) {
52  this->initialize(expression({{key1, key2}}));
53  }
54 
56  BearingRangeFactor(Key key1, Key key2, const B &measuredBearing,
57  const R &measuredRange, const SharedNoiseModel &model)
58  : Base({{key1, key2}}, model, T(measuredBearing, measuredRange)) {
59  this->initialize(expression({{key1, key2}}));
60  }
61 
62  virtual ~BearingRangeFactor() {}
63 
65  gtsam::NonlinearFactor::shared_ptr clone() const override {
66  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
67  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
68  }
69 
70  // Return measurement expression
71  Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
72  return Expression<T>(T::Measure, Expression<A1>(keys[0]),
73  Expression<A2>(keys[1]));
74  }
75 
76  Vector evaluateError(const A1& a1, const A2& a2,
77  boost::optional<Matrix&> H1 = boost::none,
78  boost::optional<Matrix&> H2 = boost::none) const
79  {
80  std::vector<Matrix> Hs(2);
81  const auto &keys = Factor::keys();
82  const Vector error = unwhitenedError(
83  {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
84  Hs);
85  if (H1) *H1 = Hs[0];
86  if (H2) *H2 = Hs[1];
87  return error;
88  }
89 
91  void print(const std::string& s = "",
92  const KeyFormatter& kf = DefaultKeyFormatter) const override {
93  std::cout << s << "BearingRangeFactor" << std::endl;
94  Base::print(s, kf);
95  }
96 
97 
98  private:
99  friend class boost::serialization::access;
100  template <class ARCHIVE>
101  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
102  ar& boost::serialization::make_nvp(
103  "Base", boost::serialization::base_object<Base>(*this));
104  }
105 }; // BearingRangeFactor
106 
108 template <typename A1, typename A2, typename B, typename R>
109 struct traits<BearingRangeFactor<A1, A2, B, R> >
110  : public Testable<BearingRangeFactor<A1, A2, B, R> > {};
111 
112 } // namespace gtsam
gtsam::ExpressionFactor< BearingRange< A1, A2 > >::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::BearingRange< A1, A2 >
BearingRange.h
Bearing-Range product.
gtsam::BearingRangeFactor::print
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Definition: BearingRangeFactor.h:91
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::BearingRangeFactor::BearingRangeFactor
BearingRangeFactor(Key key1, Key key2, const B &measuredBearing, const R &measuredRange, const SharedNoiseModel &model)
Construct from separate bearing and range.
Definition: BearingRangeFactor.h:56
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::BearingRangeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: BearingRangeFactor.h:65
gtsam::BearingRange< A1, A2 >::Measure
static BearingRange Measure(const A1 &a1, const A2 &a2, OptionalJacobian< dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< dimension, traits< A2 >::dimension > H2=boost::none)
Prediction function that stacks measurements.
Definition: BearingRange.h:77
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::BearingRangeFactor::BearingRangeFactor
BearingRangeFactor(Key key1, Key key2, const T &bearingRange, const SharedNoiseModel &model)
Construct from BearingRange instance.
Definition: BearingRangeFactor.h:49
gtsam::BearingRangeFactor
Definition: BearingRangeFactor.h:36
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::ExpressionFactorN
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition: ExpressionFactor.h:242
gtsam::ExpressionFactor< BearingRange< A1, A2 > >::initialize
void initialize(const Expression< BearingRange< A1, A2 > > &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::BearingRangeFactor::BearingRangeFactor
BearingRangeFactor()
Default constructor.
Definition: BearingRangeFactor.h:46