numerically checks getter jacobians

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -166,15 +166,40 @@ TEST(OrientedPlane3, jacobian_retract) {
   }
 }
 
+//*******************************************************************************
+TEST(OrientedPlane3, jacobian_normal) {
+  Matrix23 H_actual, H_expected;
+  OrientedPlane3 plane(-1, 0.1, 0.2, 5);
+
+  std::function<Unit3(const OrientedPlane3&)> f = std::bind(
+      &OrientedPlane3::normal, std::placeholders::_1, boost::none);
+
+  H_expected = numericalDerivative11(f, plane);
+  plane.normal(H_actual);
+  EXPECT(assert_equal(H_actual, H_expected, 1e-5));
+}
+
+//*******************************************************************************
+TEST(OrientedPlane3, jacobian_distance) {
+  Matrix13 H_actual, H_expected;
+  OrientedPlane3 plane(-1, 0.1, 0.2, 5);
+
+  std::function<double(const OrientedPlane3&)> f = std::bind(
+      &OrientedPlane3::distance, std::placeholders::_1, boost::none);
+
+  H_expected = numericalDerivative11(f, plane);
+  plane.distance(H_actual);
+  EXPECT(assert_equal(H_actual, H_expected, 1e-5));
+}
+
 //*******************************************************************************
 TEST(OrientedPlane3, getMethodJacobians) {
   OrientedPlane3 plane(-1, 0.1, 0.2, 5);
   Matrix33 H_retract, H_getters;
   Matrix23 H_normal;
   Matrix13 H_distance;
 
-  // The getter's jacobians lie exactly on the tangent space
-  // so they should exactly equal the retract jacobian for the zero vector.
+  // confirm the getters are exactly on the tangent space
   Vector3 v(0, 0, 0);
   plane.retract(v, H_retract);
   plane.normal(H_normal);