diff --git a/gtsam.h b/gtsam.h index 8097cc6c9..64f025972 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2316,6 +2316,21 @@ virtual class BearingFactor : gtsam::NoiseModelFactor { typedef gtsam::BearingFactor BearingFactor2D; typedef gtsam::BearingFactor BearingFactorPose2; +#include +template +class BearingRange { + BearingRange(const BEARING& b, const RANGE& r); + BEARING bearing() const; + RANGE range() const; + // TODO(frank): can't class instance itself? + // static gtsam::BearingRange Measure(const POSE& pose, const POINT& point); + static BEARING MeasureBearing(const POSE& pose, const POINT& point); + static RANGE MeasureRange(const POSE& pose, const POINT& point); + void print(string s) const; +}; + +typedef gtsam::BearingRange BearingRange2D; + #include template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index ffa373027..8214b0727 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -44,11 +44,11 @@ struct Range; * For example BearingRange(pose,point) will return pair * and BearingRange(pose,point) will return pair */ -template +template ::result_type, + typename R = typename Range::result_type> struct BearingRange { private: - typedef typename Bearing::result_type B; - typedef typename Range::result_type R; B bearing_; R range_; @@ -91,6 +91,16 @@ public: return BearingRange(b, r); } + /// Predict bearing + static B MeasureBearing(const A1& a1, const A2& a2) { + return Bearing()(a1, a2); + } + + /// Predict range + static R MeasureRange(const A1& a1, const A2& a2) { + return Range()(a1, a2); + } + /// @} /// @name Testable /// @{ @@ -170,8 +180,8 @@ struct HasBearing { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1, - OptionalJacobian::dimension, traits::dimension> H2) { + OptionalJacobian::dimension, traits::dimension> H1=boost::none, + OptionalJacobian::dimension, traits::dimension> H2=boost::none) { return a1.bearing(a2, H1, H2); } }; @@ -184,8 +194,8 @@ struct HasRange { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1, - OptionalJacobian::dimension, traits::dimension> H2) { + OptionalJacobian::dimension, traits::dimension> H1=boost::none, + OptionalJacobian::dimension, traits::dimension> H2=boost::none) { return a1.range(a2, H1, H2); } };