gtsam  4.1.0
gtsam
BearingFactor.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 Bearing functor, assumed partially specified
26 template <typename A1, typename A2>
27 struct Bearing;
28 
35 template <typename A1, typename A2,
36  typename T = typename Bearing<A1, A2>::result_type>
37 struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
39 
42 
44  BearingFactor(Key key1, Key key2, const T& measured,
45  const SharedNoiseModel& model)
46  : Base({key1, key2}, model, measured) {
47  this->initialize(expression({key1, key2}));
48  }
49 
50  // Return measurement expression
51  Expression<T> expression(const typename Base::ArrayNKeys &keys) const override {
52  Expression<A1> a1_(keys[0]);
53  Expression<A2> a2_(keys[1]);
54  return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
55  }
56 
58  void print(const std::string& s = "",
59  const KeyFormatter& kf = DefaultKeyFormatter) const override {
60  std::cout << s << "BearingFactor" << std::endl;
61  Base::print(s, kf);
62  }
63 
64  Vector evaluateError(const A1& a1, const A2& a2,
65  boost::optional<Matrix&> H1 = boost::none,
66  boost::optional<Matrix&> H2 = boost::none) const
67  {
68  std::vector<Matrix> Hs(2);
69  const auto &keys = Factor::keys();
70  const Vector error = unwhitenedError(
71  {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
72  Hs);
73  if (H1) *H1 = Hs[0];
74  if (H2) *H2 = Hs[1];
75  return error;
76  }
77 
78 
79  private:
80  friend class boost::serialization::access;
81  template <class ARCHIVE>
82  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
83  ar& boost::serialization::make_nvp(
84  "Base", boost::serialization::base_object<Base>(*this));
85  }
86 }; // BearingFactor
87 
89 template <typename A1, typename A2, typename T>
90 struct traits<BearingFactor<A1, A2, T> >
91  : public Testable<BearingFactor<A1, A2, T> > {};
92 
93 } // namespace gtsam
gtsam::ExpressionFactor< typename Bearing< A1, A2 >::result_type >::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< typename Bearing< A1, A2 >::result_type >::measured
const typename Bearing< A1, A2 >::result_type & measured() const
return the measurement
Definition: ExpressionFactor.h:77
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::BearingFactor
Definition: BearingFactor.h:37
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::BearingFactor::BearingFactor
BearingFactor(Key key1, Key key2, const T &measured, const SharedNoiseModel &model)
primary constructor
Definition: BearingFactor.h:44
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::ExpressionFactorN
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition: ExpressionFactor.h:242
gtsam::ExpressionFactor< typename Bearing< A1, A2 >::result_type >::initialize
void initialize(const Expression< typename Bearing< A1, A2 >::result_type > &expression)
Initialize with constructor arguments.
Definition: ExpressionFactor.h:167
gtsam::BearingFactor::BearingFactor
BearingFactor()
default constructor
Definition: BearingFactor.h:41
gtsam::BearingFactor::print
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Definition: BearingFactor.h:58
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