diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0fc02ea95..9660b86e7 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -207,6 +207,18 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + Rot2 Pose2::bearing(const Pose2& point, + boost::optional H1, boost::optional 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 H1, boost::optional H2) const { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 549963856..cc8f8848d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -157,6 +157,15 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional 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 H1=boost::none, + boost::optional H2=boost::none) const; + /** * Calculate range to a landmark * @param point 2D location of landmark diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index d498bef1f..66b0584ce 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -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));