24#include <boost/concept/assert.hpp>
25#include <boost/serialization/nvp.hpp>
33template <
typename A1,
typename A2>
39template <
typename A1,
typename A2>
48template <
typename A1,
typename A2,
59 enum { dimension = dimB + dimR };
65 BearingRange(
const B& b,
const R& r) : bearing_(b), range_(r) {}
72 const B&
bearing()
const {
return bearing_; }
75 const R&
range()
const {
return range_; }
79 const A1& a1,
const A2& a2,
82 typename MakeJacobian<B, A1>::type HB1;
83 typename MakeJacobian<B, A2>::type HB2;
84 typename MakeJacobian<R, A1>::type HR1;
85 typename MakeJacobian<R, A2>::type HR2;
90 if (H1) *H1 << HB1, HR1;
91 if (H2) *H2 << HB2, HR2;
109 void print(
const std::string& str =
"")
const {
114 bool equals(
const BearingRange<A1, A2>& m2,
double tol = 1e-8)
const {
115 return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
116 traits<R>::Equals(range_, m2.range_, tol);
123 inline static size_t Dim() {
return dimension; }
124 inline size_t dim()
const {
return dimension; }
126 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
127 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
151 template <
class ARCHIVE>
152 void serialize(ARCHIVE& ar,
const unsigned int ) {
153 ar& boost::serialization::make_nvp(
"bearing", bearing_);
154 ar& boost::serialization::make_nvp(
"range", range_);
157 friend class boost::serialization::access;
163 NeedsToAlign = (
sizeof(B) % 16) == 0 || (
sizeof(R) % 16) == 0
170template <
typename A1,
typename A2>
179template <
class A1,
typename A2,
class RT>
181 typedef RT result_type;
183 const A1& a1,
const A2& a2,
186 return a1.bearing(a2, H1, H2);
193template <
class A1,
typename A2,
class RT>
195 typedef RT result_type;
197 const A1& a1,
const A2& a2,
200 return a1.range(a2, H1, H2);
Base class and basic functions for Manifold types.
Special class for optional Jacobian arguments.
Concept check for values that can be used in unit tests.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
Global functions in a separate testing namespace.
Definition chartTesting.h:28
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
Definition BearingRange.h:34
Definition BearingRange.h:40
Bearing-Range product for a particular A1,A2 combination will use the functors above to create a simi...
Definition BearingRange.h:51
static R MeasureRange(const A1 &a1, const A2 &a2)
Predict range.
Definition BearingRange.h:101
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:78
BearingRange retract(const TangentVector &xi) const
Retract delta to manifold.
Definition BearingRange.h:130
const B & bearing() const
Return bearing measurement.
Definition BearingRange.h:72
TangentVector localCoordinates(const BearingRange &other) const
Compute the coordinates in the tangent space.
Definition BearingRange.h:137
const R & range() const
Return range measurement.
Definition BearingRange.h:75
static B MeasureBearing(const A1 &a1, const A2 &a2)
Predict bearing.
Definition BearingRange.h:96
Definition BearingRange.h:180
Definition BearingRange.h:194