From a897ee786328154abb98950c21c1a24b2175456f Mon Sep 17 00:00:00 2001
From: David <davidwisth@gmail.com>
Date: Wed, 15 Jul 2020 19:18:39 +1000
Subject: [PATCH] Fix all unit tests. Only remaining item is the analytical
 Jacobian for Unit3::localCoordinates.

---
 gtsam/geometry/OrientedPlane3.cpp           | 13 +++++++++---
 gtsam/geometry/tests/testOrientedPlane3.cpp | 23 ++++++++++++++++++++-
 gtsam/geometry/tests/testUnit3.cpp          |  1 +
 gtsam/slam/OrientedPlane3Factor.cpp         |  2 +-
 4 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp
index 76a569ffa..274d8220c 100644
--- a/gtsam/geometry/OrientedPlane3.cpp
+++ b/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);
-  if (H1) *H1 = numericalDerivative21<Vector3, OrientedPlane3, OrientedPlane3>(f, *this, plane);
-  if (H2) *H2 = numericalDerivative22<Vector3, OrientedPlane3, OrientedPlane3>(f, *this, plane);
+  const auto f = boost::bind(&Unit3::localCoordinates, _1, _2);
 
   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_);
 }
 
diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp
index dc5851319..36e566dca 100644
--- a/gtsam/geometry/tests/testOrientedPlane3.cpp
+++ b/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);
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index a25ab25c7..ada735ed4 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -492,6 +492,7 @@ TEST(actualH, Serialization) {
   EXPECT(serializationTestHelpers::equalsBinary(p));
 }
 
+
 /* ************************************************************************* */
 int main() {
   srand(time(nullptr));
diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp
index fc8bb3436..7fd225e60 100644
--- a/gtsam/slam/OrientedPlane3Factor.cpp
+++ b/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) {