From 541c1085db6dceafa2c0d53197ca840db4677869 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Thu, 6 Oct 2011 20:47:23 +0000
Subject: [PATCH] Added a range to pose function for Pose3

---
 .cproject                          |  8 ++++++
 gtsam/geometry/Pose3.cpp           | 16 ++++++++++++
 gtsam/geometry/Pose3.h             |  9 +++++++
 gtsam/geometry/tests/testPose3.cpp | 40 ++++++++++++++++++++++++++++++
 4 files changed, 73 insertions(+)

diff --git a/.cproject b/.cproject
index 9848b8ba2..e15a51f96 100644
--- a/.cproject
+++ b/.cproject
@@ -1392,6 +1392,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="tests/testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>tests/testPose3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
 			boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
+			boost::optional<Matrix&> 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 )
 {