From 6348a33165fa44b82c17363f7090a8dcc5851a8d Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 12 Jan 2011 19:31:04 +0000
Subject: [PATCH] Added range measurement function to Pose3

---
 gtsam/geometry/Pose3.cpp           | 13 +++++++++-
 gtsam/geometry/Pose3.h             |  9 +++++++
 gtsam/geometry/tests/testPose3.cpp | 40 ++++++++++++++++++++++++++++++
 3 files changed, 61 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index a85539894..8d4099933 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -245,7 +245,18 @@ namespace gtsam {
 		Pose3 result = invp1.compose(p2, composeH1, H2);
 		if (H1) *H1 = composeH1 * invH;
 		return result;
-	}
+  }
 
   /* ************************************************************************* */
+  double Pose3::range(const Point3& point,
+		  boost::optional<Matrix&> H1,
+		  boost::optional<Matrix&> H2) const {
+	  if (!H1 && !H2) return transform_to(point).norm();
+	  Point3 d = transform_to(point, H1, H2);
+	  double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
+	  Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
+	  if (H1) *H1 = D_result_d * (*H1);
+	  if (H2) *H2 = D_result_d * (*H2);
+	  return n;
+  }
 } // namespace gtsam
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 72408ab7f..271589586 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -156,6 +156,15 @@ namespace gtsam {
     			 0.,  0.,  0.,  0.);
     }
 
+	/**
+	 * Calculate range to a landmark
+	 * @param point 3D location of landmark
+	 * @return range (double)
+	 */
+	double range(const Point3& 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 a4933d6c9..9d8456e01 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -464,6 +464,46 @@ TEST( Pose3, between )
 	CHECK(assert_equal(numericalH2,actualDBetween2));
 }
 
+/* ************************************************************************* */
+// 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, 3, 0);
+Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI_4, 0.0, 0.0), l2);
+
+/* ************************************************************************* */
+LieVector range_proxy(const Pose3& pose, const Point3& point) {
+	return LieVector(pose.range(point));
+}
+TEST( Pose3, range )
+{
+	Matrix expectedH1, actualH1, expectedH2, actualH2;
+
+	// establish range is indeed zero
+	EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
+
+	// establish range is indeed sqrt2
+	EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9);
+
+	// Another pair
+	double actual23 = x2.range(l3, actualH1, actualH2);
+	EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
+
+	// Check numerical derivatives
+	expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5);
+	expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5);
+	EXPECT(assert_equal(expectedH1,actualH1));
+	EXPECT(assert_equal(expectedH2,actualH2));
+
+	// Another test
+	double actual34 = x3.range(l4, actualH1, actualH2);
+	EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
+
+	// Check numerical derivatives
+	expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5);
+	expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5);
+	EXPECT(assert_equal(expectedH1,actualH1));
+	EXPECT(assert_equal(expectedH2,actualH2));
+}
+
 /* ************************************************************************* */
 int main(){ TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */