gtsam  4.0.0
gtsam
BearingRangeFactor.h
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 ExpressionFactor2<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 B& measuredBearing,
50  const R& measuredRange, const SharedNoiseModel& model)
51  : Base(key1, key2, model, T(measuredBearing, measuredRange)) {
52  this->initialize(expression(key1, key2));
53  }
54 
55  virtual ~BearingRangeFactor() {}
56 
58  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
59  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
60  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
61  }
62 
63  // Return measurement expression
64  virtual Expression<T> expression(Key key1, Key key2) const {
66  Expression<A2>(key2));
67  }
68 
70  virtual void print(const std::string& s = "",
71  const KeyFormatter& kf = DefaultKeyFormatter) const {
72  std::cout << s << "BearingRangeFactor" << std::endl;
73  Base::print(s, kf);
74  }
75 
76 
77  private:
78  friend class boost::serialization::access;
79  template <class ARCHIVE>
80  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
81  ar& boost::serialization::make_nvp(
82  "Base", boost::serialization::base_object<Base>(*this));
83  }
84 }; // BearingRangeFactor
85 
87 template <typename A1, typename A2, typename B, typename R>
88 struct traits<BearingRangeFactor<A1, A2, B, R> >
89  : public Testable<BearingRangeFactor<A1, A2, B, R> > {};
90 
91 } // namespace gtsam
This is the base class for all factor types.
Definition: Factor.h:54
Definition: BearingRangeFactor.h:35
virtual void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const
print
Definition: BearingRangeFactor.h:70
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
Bearing-Range product.
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
Binary specialization of ExpressionFactor meant as a base class for binary factors.
Definition: ExpressionFactor.h:226
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:33
BearingRangeFactor(Key key1, Key key2, const B &measuredBearing, const R &measuredRange, const SharedNoiseModel &model)
primary constructor
Definition: BearingRangeFactor.h:49
virtual Expression< T > expression(Key key1, Key key2) const
Recreate expression from given keys_ and measured_, used in load Needed to deserialize a derived fact...
Definition: BearingRangeFactor.h:64
void initialize(const Expression< BearingRange< A1, A2 > > &expression)
Initialize with constructor arguments.
Definition: ExpressionFactor.h:157
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Expression class that supports automatic differentiation.
Definition: Expression.h:49
virtual gtsam::NonlinearFactor::shared_ptr clone() const
Definition: BearingRangeFactor.h:58
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print relies on Testable traits being defined for T
Definition: ExpressionFactor.h:70
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:1072
BearingRangeFactor()
default constructor
Definition: BearingRangeFactor.h:46