Merge pull request #362 from dwisth/fix/oriented-plane3-factor-jacobian

Fix OrientedPlane3Factor jacobian

gtsam/geometry/OrientedPlane3.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/geometry/Point2.h>
 
 #include <iostream>
+#include <gtsam/base/numericalDerivative.h>
 
 using namespace std;
 
@@ -58,8 +59,22 @@ 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.
+  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_);
 }
 
@@ -67,7 +82,7 @@ Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
 Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1,
                                     OptionalJacobian<3, 3> H2) const {
   Matrix22 H_n_error_this, H_n_error_other;
-  Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0,
+  Vector2 n_error = n_.errorVector(other.n_, H1 ? &H_n_error_this : 0,
                                     H2 ? &H_n_error_other : 0);
 
   double d_error = d_ - other.d_;

gtsam/geometry/OrientedPlane3.h

@@ -56,8 +56,8 @@ public:
   }
 
   /// Construct from a Unit3 and a distance
-  OrientedPlane3(const Unit3& s, double d) :
+  OrientedPlane3(const Unit3& n, double d) :
-    n_(s), d_(d) {
+    n_(n), d_(d) {
   }
 
   /// Construct from a vector of plane coefficients
@@ -66,8 +66,7 @@ public:
 
   /// Construct from four numbers of plane coeffcients (a, b, c, d)
   OrientedPlane3(double a, double b, double c, double d) {
-    Point3 p(a, b, c);
+    n_ = Unit3(a, b, c);
-    n_ = Unit3(p);
     d_ = d;
   }
 
@@ -112,9 +111,15 @@ public:
 
   /** Computes the error between two planes.
    *  The error is a norm 1 difference in tangent space.
-   * @param the other plane
+   * @param plane 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);
+  }
 
   /** Computes the error between the two planes, with derivatives.
    *  This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses

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

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

gtsam/slam/OrientedPlane3Factor.cpp

@@ -19,6 +19,28 @@ 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 {
+  Matrix36 predicted_H_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.error(
+      measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
+
+  // 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;
+  }
+
+  return err;
+}
+
 //***************************************************************************
 void OrientedPlane3DirectionPrior::print(const string& s,
     const KeyFormatter& keyFormatter) const {

gtsam/slam/OrientedPlane3Factor.h

@@ -47,11 +47,8 @@ public:
   /// evaluateError
   Vector evaluateError(
       const Pose3& pose, const OrientedPlane3& plane,
-      boost::optional<Matrix&> Hpose = boost::none,
+      boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> Hplane = boost::none) const override {
+      boost::optional<Matrix&> H2 = boost::none) const override;
-    auto predicted_plane = plane.transform(pose, Hplane, Hpose);
-    return predicted_plane.error(measured_p_);
-  }
 };
 
 // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior

gtsam/slam/tests/testOrientedPlane3Factor.cpp

@@ -87,7 +87,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
   // Tests one pose, two measurements of the landmark that differ in angle only.
   // Normal along -x, 3m away
   Symbol lm_sym('p', 0);
-  OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
+  OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0);
 
   ISAM2 isam2;
   Values new_values;
@@ -126,6 +126,36 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
   EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
 }
 
+TEST( OrientedPlane3Factor, Derivatives ) {
+  // Measurement
+  OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5);
+
+  // Linearisation point
+  OrientedPlane3 pLin(sqrt(3)/3, -sqrt(3)/3, sqrt(3)/3, 7);
+  gtsam::Point3 pointLin  = gtsam::Point3(1, 2, -4);
+  gtsam::Rot3 rotationLin = gtsam::Rot3::RzRyRx(0.5*M_PI, -0.3*M_PI, 1.4*M_PI);
+  Pose3 poseLin(rotationLin, pointLin);
+
+  // Factor
+  Key planeKey(1), poseKey(2);
+  SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
+  OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
+
+  // Calculate numerical derivatives
+  boost::function<Vector(const Pose3&, const OrientedPlane3&)> f = boost::bind(
+      &OrientedPlane3Factor::evaluateError, factor, _1, _2, boost::none, boost::none);
+  Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
+  Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
+
+  // Use the factor to calculate the derivative
+  Matrix actualH1, actualH2;
+  factor.evaluateError(poseLin, pLin, actualH1, actualH2);
+
+  // Verify we get the expected error
+  EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
+  EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
+}
+
 // *************************************************************************
 TEST( OrientedPlane3DirectionPrior, Constructor ) {