12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in feature/BearingRange (pull request #375) 

Wrapping BearingRange
release/4.3a0
Frank Dellaert 2019-01-26 03:27:23 +00:00
parent 3389bb2944 ca9c972028
commit a1b36dc065
2 changed files with 32 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam.h
View File

@ -2316,6 +2316,21 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
#include <gtsam/geometry/BearingRange.h>
template <POSE, POINT, BEARING, RANGE>
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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D;
#include <gtsam/sam/BearingRangeFactor.h>
template<POSE, POINT, BEARING, RANGE>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {

24
gtsam/geometry/BearingRange.h
View File

@ -44,11 +44,11 @@ struct Range;
* For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double>
* and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
*/
template <typename A1, typename A2>
template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
struct BearingRange {
private:
typedef typename Bearing<A1, A2>::result_type B;
typedef typename Range<A1, A2>::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>()(a1, a2);
}
/// Predict range
static R MeasureRange(const A1& a1, const A2& a2) {
return Range<A1, A2>()(a1, a2);
}
/// @}
/// @name Testable
/// @{
@ -170,8 +180,8 @@ struct HasBearing {
typedef RT result_type;
RT operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none,
OptionalJacobian<traits<RT>::dimension, traits<A2>::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<traits<RT>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
return a1.range(a2, H1, H2);
}
};