Add jacobian tests for retract of Unit3 & OrientedPlane3

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -161,6 +161,42 @@ TEST (OrientedPlane3, error2) {
   EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
 }
 
+//*******************************************************************************
+// Wrapper to make retract return a Vector3 so we can test numerical derivatives.
+Vector4 RetractTest(const OrientedPlane3& plane, const Vector3& v,
+                    OptionalJacobian<4, 3> H) {
+  OrientedPlane3 plane_retract = plane.retract(v, H);
+  return Vector4(plane_retract.normal().point3().x(),
+                 plane_retract.normal().point3().y(),
+                 plane_retract.normal().point3().z(),
+                 plane_retract.distance());
+}
+
+//*******************************************************************************
+TEST (OrientedPlane3, jacobian_retract) {
+  OrientedPlane3 plane(-1, 0.1, 0.2, 5);
+  Matrix43 H;
+  {
+      Vector3 v (-0.1, 0.2, 0.3);
+      plane.retract(v, H);
+      // Test that jacobian is numerically as expected.
+      boost::function<Vector4(const OrientedPlane3&, const Vector3&)> f =
+          boost::bind(RetractTest, _1, _2, boost::none);
+      Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v);
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+  }
+  {
+      Matrix43 H;
+      Vector3 v (0, 0, 0);
+      plane.retract(v, H);
+      // Test that jacobian is numerically as expected.
+      boost::function<Vector4(const OrientedPlane3&, const Vector3&)> f =
+          boost::bind(RetractTest, _1, _2, boost::none);
+      Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v);
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));

gtsam/geometry/tests/testUnit3.cpp

@@ -371,6 +371,41 @@ TEST(Unit3, retract) {
   }
 }
 
+//*******************************************************************************
+// Wrapper to make retract return a Vector3 so we can test numerical derivatives.
+Vector3 RetractTest(const Unit3&p, const Vector2& v, OptionalJacobian<3, 2> H) {
+  Unit3 p_retract = p.retract(v, H);
+  return p_retract.point3();
+}
+
+//*******************************************************************************
+TEST (OrientedPlane3, jacobian_retract) {
+  Unit3 p;
+  {
+      Vector2 v (-0.2, 0.1);
+      Matrix32 H;
+      p.retract(v, H);
+
+      // Test that jacobian is numerically as expected.
+      boost::function<Vector3(const Unit3&, const Vector2&)> f =
+          boost::bind(RetractTest, _1, _2, boost::none);
+      Matrix32 H_expected_numerical = numericalDerivative22(f, p, v);
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+  }
+  {
+      Vector2 v (0, 0);
+      Matrix32 H;
+      p.retract(v, H);
+
+      // Test that jacobian is numerically as expected.
+      boost::function<Vector3(const Unit3&, const Vector2&)> f =
+          boost::bind(RetractTest, _1, _2, boost::none);
+      Matrix32 H_expected_numerical = numericalDerivative22(f, p, v);
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+
+  }
+}
+
 //*******************************************************************************
 TEST(Unit3, retract_expmap) {
   Unit3 p;