Fix/plane factor

gtsam/geometry/OrientedPlane3.cpp

@@ -18,7 +18,7 @@
  */
 
 #include <gtsam/geometry/OrientedPlane3.h>
-#include <gtsam/geometry/Point2.h>
+
 #include <iostream>
 
 using namespace std;
@@ -28,12 +28,13 @@ namespace gtsam {
 /* ************************************************************************* */
 void OrientedPlane3::print(const string& s) const {
   Vector4 coeffs = planeCoefficients();
-  cout << s << " : " << coeffs << endl;
+  cout << s << " : " << coeffs.transpose() << endl;
 }
 
 /* ************************************************************************* */
-OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp,
-    OptionalJacobian<3, 6> Hr) const {
+OrientedPlane3 OrientedPlane3::transform(const Pose3& xr,
+                                         OptionalJacobian<3, 3> Hp,
+                                         OptionalJacobian<3, 6> Hr) const {
   Matrix23 D_rotated_plane;
   Matrix22 D_rotated_pose;
   Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
@@ -42,53 +43,47 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
   double pred_d = n_.unitVector().dot(xr.translation()) + d_;
 
   if (Hr) {
-    *Hr = Matrix::Zero(3,6);
+    Hr->setZero();
     Hr->block<2, 3>(0, 0) = D_rotated_plane;
     Hr->block<1, 3>(2, 3) = unit_vec;
   }
   if (Hp) {
-    Vector2 hpp = n_.basis().transpose() * xr.translation();
-    *Hp = Z_3x3;
+    Hp->setZero();
     Hp->block<2, 2>(0, 0) = D_rotated_pose;
-    Hp->block<1, 2>(2, 0) = hpp;
+    Hp->block<1, 2>(2, 0) = n_.basis().transpose() * xr.translation();
     (*Hp)(2, 2) = 1;
   }
 
   return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
 }
 
 /* ************************************************************************* */
-Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
-  Vector2 n_error = -n_.localCoordinates(plane.n_);
-  return Vector3(n_error(0), n_error(1), d_ - plane.d_);
-}
-
-/* ************************************************************************* */
-Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1,
+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_;
 
   if (H1) {
-    *H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1;
+    *H1 << H_n_error_this, Z_2x1, 0, 0, 1;
   }
   if (H2) {
-    *H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1;
+    *H2 << H_n_error_other, Z_2x1, 0, 0, -1;
   }
 
   return Vector3(n_error(0), n_error(1), d_error);
 }
 
 /* ************************************************************************* */
 OrientedPlane3 OrientedPlane3::retract(const Vector3& v,
-                                       OptionalJacobian<3,3> H) const {
+                                       OptionalJacobian<3, 3> H) const {
   Matrix22 H_n;
-  Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
+  Unit3 n_retract(n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
   if (H) {
-    *H << H_n, Vector2::Zero(), 0, 0, 1;
+    *H << H_n, Z_2x1, 0, 0, 1;
   }
   return OrientedPlane3(n_retract, d_ + v(2));
 }
@@ -100,4 +95,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
   return Vector3(n_local(0), n_local(1), -d_local);
 }
 
-}
+}  // namespace gtsam

gtsam/geometry/OrientedPlane3.h

@@ -20,22 +20,21 @@
 
 #pragma once
 
-#include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Pose3.h>
+#include <string>
 
 namespace gtsam {
 
 /**
- * @brief Represents an infinite plane in 3D, which is composed of a planar normal and its
- *  perpendicular distance to the origin.
- * Currently it provides a transform of the plane, and a norm 1 differencing of two planes.
+ * @brief Represents an infinite plane in 3D, which is composed of a planar
+ * normal and its perpendicular distance to the origin.
+ * Currently it provides a transform of the plane, and a norm 1 differencing of
+ * two planes.
  * Refer to Trevor12iros for more math details.
  */
 class GTSAM_EXPORT OrientedPlane3 {
-
 private:
-
   Unit3 n_;     ///< The direction of the planar normal
   double d_;    ///< The perpendicular distance to this plane
 
@@ -53,19 +52,17 @@ class GTSAM_EXPORT OrientedPlane3 {
   }
 
   /// Construct from a Unit3 and a distance
-  OrientedPlane3(const Unit3& s, double d) :
-    n_(s), d_(d) {
+  OrientedPlane3(const Unit3& n, double d) :
+    n_(n), d_(d) {
   }
 
   /// Construct from a vector of plane coefficients
-  OrientedPlane3(const Vector4& vec) :
-    n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
-  }
+  explicit OrientedPlane3(const Vector4& vec)
+      : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {}
 
   /// 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(p);
+    n_ = Unit3(a, b, c);
     d_ = d;
   }
 
@@ -90,37 +87,18 @@ class GTSAM_EXPORT OrientedPlane3 {
    * @return the transformed plane
    */
   OrientedPlane3 transform(const Pose3& xr,
-      OptionalJacobian<3, 3> Hp = boost::none,
-      OptionalJacobian<3, 6> Hr = boost::none) const;
-
-  /**
-   * @deprecated the static method has wrong Jacobian order,
-   *    please use the member method transform()
-   * @param The raw plane
-   * @param xr a transformation in current coordiante
-   * @param Hr optional jacobian wrpt the pose transformation
-   * @param Hp optional Jacobian wrpt the destination plane
-   * @return the transformed plane
-   */
-  static OrientedPlane3 Transform(const OrientedPlane3& plane,
-      const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
-      OptionalJacobian<3, 3> Hp = boost::none) {
-      return plane.transform(xr, Hp, Hr);
-  }
-
-  /** Computes the error between two planes.
-   *  The error is a norm 1 difference in tangent space.
-   * @param the other plane
-   */
-  Vector3 error(const OrientedPlane3& plane) const;
+                           OptionalJacobian<3, 3> Hp = boost::none,
+                           OptionalJacobian<3, 6> Hr = boost::none) const;
 
   /** Computes the error between the two planes, with derivatives.
-   *  This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
-   *  Unit3::localCoordinates. This one has correct derivatives.
+   *  This uses Unit3::errorVector, as opposed to the other .error() in this
+   *  class, which uses Unit3::localCoordinates. This one has correct
+   *  derivatives.
    *  NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
-   * @param the other plane
+   * @param other the other plane
    */
-  Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
+  Vector3 errorVector(const OrientedPlane3& other,
+                      OptionalJacobian<3, 3> H1 = boost::none,
                       OptionalJacobian<3, 3> H2 = boost::none) const;
 
   /// Dimensionality of tangent space = 3 DOF
@@ -134,7 +112,8 @@ class GTSAM_EXPORT OrientedPlane3 {
   }
 
   /// The retract function
-  OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const;
+  OrientedPlane3 retract(const Vector3& v,
+                        OptionalJacobian<3, 3> H = boost::none) const;
 
   /// The local coordinates function
   Vector3 localCoordinates(const OrientedPlane3& s) const;
@@ -166,5 +145,5 @@ template<> struct traits<const OrientedPlane3> : public internal::Manifold<
 OrientedPlane3> {
 };
 
-} // namespace gtsam
+}  // namespace gtsam
 

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -31,7 +31,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
 GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
 
 //*******************************************************************************
-TEST (OrientedPlane3, getMethods) {
+TEST(OrientedPlane3, getMethods) {
   Vector4 c;
   c << -1, 0, 0, 5;
   OrientedPlane3 plane1(c);
@@ -50,44 +50,27 @@ TEST (OrientedPlane3, getMethods) {
 
 
 //*******************************************************************************
-OrientedPlane3 Transform_(const OrientedPlane3& plane,  const Pose3& xr) {
-  return OrientedPlane3::Transform(plane, xr);
-}
-
 OrientedPlane3 transform_(const OrientedPlane3& plane,  const Pose3& xr) {
   return plane.transform(xr);
 }
 
-TEST (OrientedPlane3, transform) {
+TEST(OrientedPlane3, transform) {
   gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0),
-      gtsam::Point3(2.0, 3.0, 4.0));
+                    gtsam::Point3(2.0, 3.0, 4.0));
   OrientedPlane3 plane(-1, 0, 0, 5);
   OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
-  OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
-      none, none);
-  OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
-  EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-5));
-  EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-5));
+  OrientedPlane3 transformedPlane = plane.transform(pose, none, none);
+  EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5));
 
   // Test the jacobians of transform
   Matrix actualH1, expectedH1, actualH2, expectedH2;
-  {
-    // because the Transform class uses a wrong order of Jacobians in interface
-    OrientedPlane3::Transform(plane, pose, actualH1, none);
-    expectedH1 = numericalDerivative22(Transform_, plane, pose);
-    EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
-    OrientedPlane3::Transform(plane, pose, none, actualH2);
-    expectedH2 = numericalDerivative21(Transform_, plane, pose);
-    EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
-  }
-  {
-    plane.transform(pose, actualH1, none);
-    expectedH1 = numericalDerivative21(transform_, plane, pose);
-    EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
-    plane.transform(pose, none, actualH2);
-    expectedH2 = numericalDerivative22(Transform_, plane, pose);
-    EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
-  }
+  expectedH1 = numericalDerivative21(transform_, plane, pose);
+  plane.transform(pose, actualH1, none);
+  EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
+
+  expectedH2 = numericalDerivative22(transform_, plane, pose);
+  plane.transform(pose, none, actualH2);
+  EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
 }
 
 //*******************************************************************************
@@ -109,7 +92,6 @@ inline static Vector randomVector(const Vector& minLimits,
 
 //*******************************************************************************
 TEST(OrientedPlane3, localCoordinates_retract) {
-
   size_t numIterations = 10000;
   Vector4 minPlaneLimit, maxPlaneLimit;
   minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
@@ -119,7 +101,6 @@ TEST(OrientedPlane3, localCoordinates_retract) {
   minXiLimit << -M_PI, -M_PI, -10.0;
   maxXiLimit << M_PI, M_PI, 10.0;
   for (size_t i = 0; i < numIterations; i++) {
-
     // Create a Plane
     OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit));
     Vector v12 = randomVector(minXiLimit, maxXiLimit);
@@ -138,22 +119,24 @@ 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);
 
   // Hard-coded regression values, to ensure the result doesn't change.
   EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8));
-  EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5));
+  EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4),
+                      plane1.errorVector(plane2), 1e-5));
 
   // Test the jacobians of transform
   Matrix33 actualH1, expectedH1, actualH2, expectedH2;
 
   Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2);
-  EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1])));
+  EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()),
+                      Vector2(actual[0], actual[1])));
   EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2]));
 
-  boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f = //
+  boost::function<Vector3(const OrientedPlane3&, const OrientedPlane3&)> f =
       boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none);
   expectedH1 = numericalDerivative21(f, plane1, plane2);
   expectedH2 = numericalDerivative22(f, plane1, plane2);
@@ -162,19 +145,19 @@ TEST (OrientedPlane3, error2) {
 }
 
 //*******************************************************************************
-TEST (OrientedPlane3, jacobian_retract) {
+TEST(OrientedPlane3, jacobian_retract) {
   OrientedPlane3 plane(-1, 0.1, 0.2, 5);
   Matrix33 H_actual;
   boost::function<OrientedPlane3(const Vector3&)> f =
       boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
   {
-      Vector3 v (-0.1, 0.2, 0.3);
+      Vector3 v(-0.1, 0.2, 0.3);
       plane.retract(v, H_actual);
       Matrix H_expected_numerical = numericalDerivative11(f, v);
       EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));
   }
   {
-      Vector3 v (0, 0, 0);
+      Vector3 v(0, 0, 0);
       plane.retract(v, H_actual);
       Matrix H_expected_numerical = numericalDerivative11(f, v);
       EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-5));

gtsam/geometry/tests/testUnit3.cpp

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