24 #include <boost/concept/assert.hpp> 32 template <
typename A1,
typename A2>
38 template <
typename A1,
typename A2>
47 template <
typename A1,
typename A2,
78 const A1& a1,
const A2& a2,
89 if (H1) *H1 << HB1, HR1;
90 if (H2) *H2 << HB2, HR2;
150 template <
class ARCHIVE>
152 ar& boost::serialization::make_nvp(
"bearing", bearing_);
153 ar& boost::serialization::make_nvp(
"range", range_);
169 template <
typename A1,
typename A2>
178 template <
class A1,
typename A2,
class RT>
182 const A1& a1,
const A2& a2,
185 return a1.bearing(a2, H1, H2);
192 template <
class A1,
typename A2,
class RT>
196 const A1& a1,
const A2& a2,
199 return a1.range(a2, H1, H2);
const R & range() const
Return range measurement.
static B MeasureBearing(const A1 &a1, const A2 &a2)
Predict bearing.
Concept check for values that can be used in unit tests.
BearingRange retract(const TangentVector &xi) const
Retract delta to manifold.
Rot2 R(Rot2::fromAngle(0.1))
TangentVector localCoordinates(const BearingRange &other) const
Compute the coordinates in the tangent space.
OptionalJacobian< dimension, dimension > ChartJacobian
Eigen::Matrix< double, dimension, 1 > TangentVector
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
void print(const std::string &str="") const
bool equals(const BearingRange< A1, A2 > &m2, double tol=1e-8) const
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.
Base class and basic functions for Manifold types.
RT operator()(const A1 &a1, const A2 &a2, OptionalJacobian< traits< RT >::dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< traits< RT >::dimension, traits< A2 >::dimension > H2=boost::none)
friend class boost::serialization::access
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RT operator()(const A1 &a1, const A2 &a2, OptionalJacobian< traits< RT >::dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< traits< RT >::dimension, traits< A2 >::dimension > H2=boost::none)
BearingRange(const B &b, const R &r)
void serialize(ARCHIVE &ar, const unsigned int)
Serialization function.
const B & bearing() const
Return bearing measurement.
Special class for optional Jacobian arguments.
The matrix class, also used for vectors and row-vectors.
static R MeasureRange(const A1 &a1, const A2 &a2)
Predict range.
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)