diff --git a/.cproject b/.cproject
index 9848b8ba2..e15a51f96 100644
--- a/.cproject
+++ b/.cproject
@@ -1392,6 +1392,14 @@
true
true
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
make
-j2
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index a3234f172..a0873db48 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -272,4 +272,20 @@ namespace gtsam {
if (H2) *H2 = D_result_d * (*H2);
return n;
}
+
+ /* ************************************************************************* */
+ double Pose3::range(const Pose3& point,
+ boost::optional H1, boost::optional H2) const {
+ double r = range(point.translation(), H1, H2);
+ if (H2) {
+#ifdef CORRECT_POSE3_EXMAP
+ Matrix H2_ = *H2 * point.rotation().matrix();
+#else
+ Matrix H2_ = *H2;
+#endif
+ *H2 = zeros(1, 6);
+ insertSub(*H2, H2_, 0, 3);
+ }
+ return r;
+ }
} // namespace gtsam
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 997d9ed36..17c61230a 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -172,6 +172,15 @@ namespace gtsam {
boost::optional H1=boost::none,
boost::optional H2=boost::none) const;
+ /**
+ * Calculate range to another pose
+ * @param point SO(3) pose of landmark
+ * @return range (double)
+ */
+ double range(const Pose3& point,
+ boost::optional H1=boost::none,
+ boost::optional H2=boost::none) const;
+
private:
/** Serialization function */
friend class boost::serialization::access;
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 3f3e58ba0..fd252d82e 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -570,6 +570,11 @@ TEST( Pose3, between )
// some shared test values - pulled from equivalent test in Pose2
Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4);
Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI_4, 0.0, 0.0), l2);
+Pose3
+ xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)),
+ xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
+ xl3(Rot3::ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)),
+ xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
/* ************************************************************************* */
LieVector range_proxy(const Pose3& pose, const Point3& point) {
@@ -606,6 +611,41 @@ TEST( Pose3, range )
EXPECT(assert_equal(expectedH2,actualH2));
}
+/* ************************************************************************* */
+LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) {
+ return LieVector(pose.range(point));
+}
+TEST( Pose3, range_pose )
+{
+ Matrix expectedH1, actualH1, expectedH2, actualH2;
+
+ // establish range is indeed zero
+ EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
+
+ // establish range is indeed sqrt2
+ EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9);
+
+ // Another pair
+ double actual23 = x2.range(xl3, actualH1, actualH2);
+ EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
+
+ // Check numerical derivatives
+ expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
+ expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3);
+ EXPECT(assert_equal(expectedH1,actualH1));
+ EXPECT(assert_equal(expectedH2,actualH2));
+
+ // Another test
+ double actual34 = x3.range(xl4, actualH1, actualH2);
+ EXPECT_DOUBLES_EQUAL(5,actual34,1e-9);
+
+ // Check numerical derivatives
+ expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4);
+ expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4);
+ EXPECT(assert_equal(expectedH1,actualH1));
+ EXPECT(assert_equal(expectedH2,actualH2));
+}
+
/* ************************************************************************* */
TEST( Pose3, unicycle )
{