Fix all unit tests. Only remaining item is the analytical Jacobian for Unit3::localCoordinates.

gtsam/geometry/OrientedPlane3.cpp

@@ -63,11 +63,18 @@ Vector3 OrientedPlane3::error(const OrientedPlane3& plane,
                               OptionalJacobian<3,3> H1,
                               OptionalJacobian<3,3> H2) const {
   // Numerically calculate the derivative since this function doesn't provide one.
-  auto f = boost::bind(&OrientedPlane3::Error, _1, _2);
+  const auto f = boost::bind(&Unit3::localCoordinates, _1, _2);
-  if (H1) *H1 = numericalDerivative21<Vector3, OrientedPlane3, OrientedPlane3>(f, *this, plane);
-  if (H2) *H2 = numericalDerivative22<Vector3, OrientedPlane3, OrientedPlane3>(f, *this, plane);
 
   Vector2 n_error = -n_.localCoordinates(plane.n_);
+
+  if (H1) {
+    *H1 = I_3x3;
+    H1->block<2,2>(0,0) = -numericalDerivative21<Vector2, Unit3, Unit3>(f, n_, plane.n_);;
+  }
+  if (H2) {
+    *H2 = -I_3x3;
+    H2->block<2,2>(0,0) = -numericalDerivative22<Vector2, Unit3, Unit3>(f, n_, plane.n_);;
+  }
   return Vector3(n_error(0), n_error(1), d_ - plane.d_);
 }
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -138,7 +138,7 @@ TEST(OrientedPlane3, localCoordinates_retract) {
 }
 
 //*******************************************************************************
-TEST (OrientedPlane3, error2) {
+TEST (OrientedPlane3, errorVector) {
   OrientedPlane3 plane1(-1, 0.1, 0.2, 5);
   OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
 
@@ -161,6 +161,27 @@ TEST (OrientedPlane3, error2) {
   EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
 }
 
+//*******************************************************************************
+TEST (OrientedPlane3, error) {
+  // Hard-coded regression values, to ensure the result doesn't change.
+  OrientedPlane3 plane1(-1, 0.1, 0.2, 5);
+  OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
+
+  // Test the jacobians of transform
+  Matrix33 actualH1, expectedH1, actualH2, expectedH2;
+  Vector3 actual = plane1.error(plane2, actualH1, actualH2);
+
+  EXPECT(assert_equal((Vector) Z_3x1, plane1.error(plane1), 1e-8));
+  EXPECT(assert_equal(Vector3(0.0678852, 0.0761865, -0.4), actual, 1e-5));
+
+  boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = //
+      boost::bind(&OrientedPlane3::error, _1, _2, boost::none, boost::none);
+  expectedH1 = numericalDerivative21(f, plane1, plane2);
+  expectedH2 = numericalDerivative22(f, plane1, plane2);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
+}
+
 //*******************************************************************************
 TEST (OrientedPlane3, jacobian_retract) {
   OrientedPlane3 plane(-1, 0.1, 0.2, 5);

gtsam/geometry/tests/testUnit3.cpp

@@ -492,6 +492,7 @@ TEST(actualH, Serialization) {
   EXPECT(serializationTestHelpers::equalsBinary(p));
 }
 
+
 /* ************************************************************************* */
 int main() {
   srand(time(nullptr));

gtsam/slam/OrientedPlane3Factor.cpp

@@ -27,7 +27,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
   Matrix33 predicted_H_plane, error_H_predicted;
   OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr,
                                                          H1 ? &predicted_H_pose  : nullptr);
-  Vector3 err = predicted_plane.errorVector(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
+  Vector3 err = predicted_plane.error(measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
 
   // Apply the chain rule to calculate the derivatives.
   if (H1) {