From 6162ce6fa14bc95025a155daf79b05bb7a3d2900 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 5 Apr 2013 00:11:19 +0000
Subject: [PATCH] Another unit test for between derivatives

---
 gtsam/geometry/tests/testPose2.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index c2c225ee3..1b73318ec 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -457,6 +457,21 @@ TEST( Pose2, between2 )
   EXPECT(assert_equal(numericalH2,actualH2));
 }
 
+/* ************************************************************************* */
+// arbitrary, non perpendicular angles to be extra safe
+TEST( Pose2, between3 )
+{
+  Pose2 p2(M_PI/3.0, Point2(1,2));
+  Pose2 p1(M_PI/6.0, Point2(-1,4));
+
+  Matrix actualH1,actualH2;
+  p1.between(p2,actualH1,actualH2);
+  Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2);
+  EXPECT(assert_equal(numericalH1,actualH1));
+  Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2);
+  EXPECT(assert_equal(numericalH2,actualH2));
+}
+
 /* ************************************************************************* */
 TEST( Pose2, round_trip )
 {