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

Added a bearing() to pose function

release/4.3a0
Alex Cunningham 2011-10-06 20:47:16 +00:00
parent 131482298d
commit 3f9feea089
3 changed files with 59 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Pose2.cpp
View File

@ -207,6 +207,18 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 result = bearing(point.t(), H1, H2);
if (H2) {
Matrix H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
}
return result;
}
/* ************************************************************************* */
double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

9
gtsam/geometry/Pose2.h
View File

@ -157,6 +157,15 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate bearing to another pose
* @param point SO(2) location of other pose
* @return 2D rotation \in SO(2)
*/
Rot2 bearing(const Pose2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to a landmark
* @param point 2D location of landmark

38
gtsam/geometry/tests/testPose2.cpp
View File

@ -504,6 +504,44 @@ TEST( Pose2, bearing )
EXPECT(assert_equal(expectedH1,actualH1));
}
/* ************************************************************************* */
Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
return pose.bearing(pt);
}
TEST( Pose2, bearing_pose )
{
Pose2 xl1(1, 0, M_PI_2), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI_2), xl4(1, 3, 0);
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish bearing is indeed zero
EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));
// establish bearing is indeed 45 degrees
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(xl2)));
// establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
expectedH2 = numericalDerivative22(bearing_pose_proxy, x2, xl3);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
// establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
expectedH2 = numericalDerivative22(bearing_pose_proxy, x3, xl4);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */
LieVector range_proxy(const Pose2& pose, const Point2& point) {
return LieVector(pose.range(point));