gtsam  4.1.0
gtsam
BearingRange.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/base/Manifold.h>
22 #include <gtsam/base/Testable.h>
24 #include <boost/concept/assert.hpp>
25 #include <iostream>
26 
27 namespace gtsam {
28 
29 // Forward declaration of Bearing functor which should be of A1*A2 -> return_type
30 // For example Bearing<Pose3,Point3>(pose,point), defined in Pose3.h will return Unit3
31 // At time of writing only Pose2 and Pose3 specialize this functor.
32 template <typename A1, typename A2>
33 struct Bearing;
34 
35 // Forward declaration of Range functor which should be of A1*A2 -> return_type
36 // For example Range<Pose2,Pose2>(T1,T2), defined in Pose2.h will return double
37 // At time of writing Pose2, Pose3, and several Camera variants specialize this for several types
38 template <typename A1, typename A2>
39 struct Range;
40 
47 template <typename A1, typename A2,
48  typename B = typename Bearing<A1, A2>::result_type,
49  typename R = typename Range<A1, A2>::result_type>
50 struct BearingRange {
51 private:
52  B bearing_;
53  R range_;
54 
55 public:
56  enum { dimB = traits<B>::dimension };
57  enum { dimR = traits<R>::dimension };
58  enum { dimension = dimB + dimR };
59 
62 
63  BearingRange() {}
64  BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
65 
69 
71  const B& bearing() const { return bearing_; }
72 
74  const R& range() const { return range_; }
75 
78  const A1& a1, const A2& a2,
79  OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
80  OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
81  typename MakeJacobian<B, A1>::type HB1;
82  typename MakeJacobian<B, A2>::type HB2;
83  typename MakeJacobian<R, A1>::type HR1;
84  typename MakeJacobian<R, A2>::type HR2;
85 
86  B b = Bearing<A1, A2>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
87  R r = Range<A1, A2>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
88 
89  if (H1) *H1 << HB1, HR1;
90  if (H2) *H2 << HB2, HR2;
91  return BearingRange(b, r);
92  }
93 
95  static B MeasureBearing(const A1& a1, const A2& a2) {
96  return Bearing<A1, A2>()(a1, a2);
97  }
98 
100  static R MeasureRange(const A1& a1, const A2& a2) {
101  return Range<A1, A2>()(a1, a2);
102  }
103 
107 
108  void print(const std::string& str = "") const {
109  std::cout << str;
110  traits<B>::Print(bearing_, "bearing ");
111  traits<R>::Print(range_, "range ");
112  }
113  bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
114  return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
115  traits<R>::Equals(range_, m2.range_, tol);
116  }
117 
121 
122  inline static size_t Dim() { return dimension; }
123  inline size_t dim() const { return dimension; }
124 
125  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
126  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
127 
129  BearingRange retract(const TangentVector& xi) const {
130  B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
131  R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
132  return BearingRange(m1, m2);
133  }
134 
136  TangentVector localCoordinates(const BearingRange& other) const {
137  typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
138  typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
139  TangentVector v;
140  v << v1, v2;
141  return v;
142  }
143 
147 
148 private:
150  template <class ARCHIVE>
151  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
152  ar& boost::serialization::make_nvp("bearing", bearing_);
153  ar& boost::serialization::make_nvp("range", range_);
154  }
155 
156  friend class boost::serialization::access;
157 
159 
160  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
161  enum {
162  NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
163  };
164 public:
166 };
167 
168 // Declare this to be both Testable and a Manifold
169 template <typename A1, typename A2>
170 struct traits<BearingRange<A1, A2> >
171  : Testable<BearingRange<A1, A2> >,
172  internal::ManifoldTraits<BearingRange<A1, A2> > {};
173 
174 // Helper class for to implement Range traits for classes with a bearing method
175 // For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say
176 // template <> struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
177 // where the third argument is used to indicate the return type
178 template <class A1, typename A2, class RT>
179 struct HasBearing {
180  typedef RT result_type;
181  RT operator()(
182  const A1& a1, const A2& a2,
185  return a1.bearing(a2, H1, H2);
186  }
187 };
188 
189 // Similar helper class for to implement Range traits for classes with a range method
190 // For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
191 // template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
192 template <class A1, typename A2, class RT>
193 struct HasRange {
194  typedef RT result_type;
195  RT operator()(
196  const A1& a1, const A2& a2,
199  return a1.range(a2, H1, H2);
200  }
201 };
202 
203 } // namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
gtsam::HasRange
Definition: BearingRange.h:193
gtsam::BearingRange
Bearing-Range product for a particular A1,A2 combination will use the functors above to create a simi...
Definition: BearingRange.h:50
gtsam::BearingRange::retract
BearingRange retract(const TangentVector &xi) const
Retract delta to manifold.
Definition: BearingRange.h:129
gtsam::Bearing
Definition: BearingRange.h:33
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
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::HasBearing
Definition: BearingRange.h:179
gtsam::internal::ManifoldTraits
A helper that implements the traits interface for GTSAM manifolds.
Definition: Manifold.h:95
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam::BearingRange::MeasureRange
static R MeasureRange(const A1 &a1, const A2 &a2)
Predict range.
Definition: BearingRange.h:100
gtsam::BearingRange::localCoordinates
TangentVector localCoordinates(const BearingRange &other) const
Compute the coordinates in the tangent space.
Definition: BearingRange.h:136
gtsam::BearingRange::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::BearingRange::range
const R & range() const
Return range measurement.
Definition: BearingRange.h:74
gtsam::BearingRange::MeasureBearing
static B MeasureBearing(const A1 &a1, const A2 &a2)
Predict bearing.
Definition: BearingRange.h:95
gtsam::Range
Definition: BearingRange.h:39
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition: types.h:278
Manifold.h
Base class and basic functions for Manifold types.
gtsam::BearingRange::bearing
const B & bearing() const
Return bearing measurement.
Definition: BearingRange.h:71