Address Frank's comments and clean up changes

gtsam/geometry/OrientedPlane3.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/geometry/OrientedPlane3.h>
 #include <gtsam/geometry/Point2.h>
 #include <iostream>
+#include <gtsam/base/numericalDerivative.h>
 
 using namespace std;
 
@@ -58,7 +59,14 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
 }
 
 /* ************************************************************************* */
-Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
+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);
+
   Vector2 n_error = -n_.localCoordinates(plane.n_);
   return Vector3(n_error(0), n_error(1), d_ - plane.d_);
 }

gtsam/geometry/OrientedPlane3.h

@@ -112,7 +112,9 @@ public:
    *  The error is a norm 1 difference in tangent space.
    * @param the other plane
    */
-  Vector3 error(const OrientedPlane3& plane) const;
+  Vector3 error(const OrientedPlane3& plane,
+                OptionalJacobian<3,3> H1 = boost::none,
+                OptionalJacobian<3,3> H2 = boost::none) const;
 
   static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) {
     return plane1.error(plane2);

gtsam/slam/OrientedPlane3Factor.cpp

@@ -19,6 +19,33 @@ void OrientedPlane3Factor::print(const string& s,
   this->noiseModel_->print("  noise model: ");
 }
 
+//***************************************************************************
+Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
+    const OrientedPlane3& plane, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+  Vector err(3);
+
+  if (H1 || H2) {
+    Matrix36 predicted_H_pose;
+    Matrix33 predicted_H_plane, error_H_predicted;
+
+    OrientedPlane3 predicted_plane = plane.transform(pose, predicted_H_plane, predicted_H_pose);
+    err << predicted_plane.error(measured_p_, error_H_predicted);
+
+    // Apply the chain rule to calculate the derivatives.
+    if (H1) {
+      *H1 = error_H_predicted * predicted_H_pose;
+    }
+    if (H2) {
+      *H2 = error_H_predicted * predicted_H_plane;
+    }
+  } else {
+    OrientedPlane3 predicted_plane = plane.transform(pose);
+    err << predicted_plane.error(measured_p_);
+  }
+  return (err);
+}
+
 //***************************************************************************
 void OrientedPlane3DirectionPrior::print(const string& s,
     const KeyFormatter& keyFormatter) const {

gtsam/slam/OrientedPlane3Factor.h

@@ -7,7 +7,6 @@
 
 #pragma once
 
-#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/OrientedPlane3.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
@@ -48,31 +47,7 @@ public:
   /// evaluateError
   virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
+          boost::none) const;
-    Vector err(3);
-
-    if (H1 || H2) {
-      Matrix H1_1, H2_1;
-      OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1);
-      err << predicted_plane.error(measured_p_);
-      // Numerically calculate the derivative since this function doesn't provide one.
-      auto f = boost::bind(&OrientedPlane3::Error, _1, _2);
-      Matrix H1_2 = numericalDerivative21<Vector3, OrientedPlane3, OrientedPlane3>(f, predicted_plane, measured_p_);
-
-      // Apply the chain rule to calculate the derivatives.
-      if (H1) {
-        *H1 = H1_2 * H1_1;
-      }
-      if (H2) {
-        *H2 = H1_2 * H2_1;
-      }
-    } else {
-      OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, H2);
-      err << predicted_plane.error(measured_p_);
-    }
-    return (err);
-  }
-  ;
 };
 
 // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior

gtsam/slam/tests/testOrientedPlane3Factor.cpp

@@ -122,40 +122,34 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
   EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
 }
 
-double randDouble(double max = 1) {
-  return static_cast<double>(rand()) / RAND_MAX * max;
-}
-
 TEST( OrientedPlane3Factor, Derivatives ) {
-  for (int i=0; i<100; i++) {
+  // Measurement
-    // Random measurement
+  OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5);
-    OrientedPlane3 p(randDouble(), randDouble(), randDouble(), randDouble());
 
-    // Random linearisation point
+  // Linearisation point
-    OrientedPlane3 pLin(randDouble(), randDouble(), randDouble(), randDouble());
+  OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7);
-    gtsam::Point3 pointLin  = gtsam::Point3(randDouble(100), randDouble(100), randDouble(100));
+  gtsam::Point3 pointLin  = gtsam::Point3(1, 2, -4);
-    gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(randDouble(2*M_PI), randDouble(2*M_PI), randDouble(2*M_PI));
+  gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI);
-    Pose3 poseLin(rotationLin, pointLin);
+  Pose3 poseLin(rotationLin, pointLin);
 
-    // Factor
+  // Factor
-    Key planeKey(1), poseKey(2);
+  Key planeKey(1), poseKey(2);
-    SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
-    OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
+  OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
 
-    // Calculate numerical derivatives
+  // Calculate numerical derivatives
-    boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
+  boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
-        &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
+      &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
-    Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
+  Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
-    Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
+  Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
 
-    // Use the factor to calculate the derivative
+  // Use the factor to calculate the derivative
-    Matrix actualH1, actualH2;
+  Matrix actualH1, actualH2;
-    factor.evaluateError(poseLin, pLin, actualH1, actualH2);
+  factor.evaluateError(poseLin, pLin, actualH1, actualH2);
 
-    // Verify we get the expected error
+  // Verify we get the expected error
-    EXPECT(assert_equal(numericalH1, actualH1, 1e-7));
+  EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
-    EXPECT(assert_equal(numericalH2, actualH2, 1e-7));
+  EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
-  }
 }
 
 // *************************************************************************