change: add transform member class, and deprecate the static Transform class

gtsam/geometry/OrientedPlane3.cpp

@@ -49,7 +49,7 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane,
   }
   if (Hp) {
     Vector2 hpp = plane.n_.basis().transpose() * xr.translation().vector();
-    *Hp = zeros(3, 3);
+    *Hp = Z_3x3;
     Hp->block<2, 2>(0, 0) = n_hp;
     Hp->block<1, 2>(2, 0) = hpp;
     (*Hp)(2, 2) = 1;
@@ -58,26 +58,48 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane,
   return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
 }
 
+/* ************************************************************************* */
+OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp,
+    OptionalJacobian<3, 6> Hr) const {
+  Matrix23 n_hr;
+  Matrix22 n_hp;
+  Unit3 n_rotated = xr.rotation().unrotate(n_, n_hr, n_hp);
+
+  Vector3 unit_vec = n_rotated.unitVector();
+  double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
+
+  if (Hr) {
+    *Hr = zeros(3, 6);
+    Hr->block<2, 3>(0, 0) = n_hr;
+    Hr->block<1, 3>(2, 3) = unit_vec;
+  }
+  if (Hp) {
+    Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
+    *Hp = Z_3x3;
+    Hp->block<2, 2>(0, 0) = n_hp;
+    Hp->block<1, 2>(2, 0) = hpp;
+    (*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_);
-  double d_error = d_ - plane.d_;
+  return Vector3(n_error(0), n_error(1), d_ - plane.d_);
-  return Vector3(n_error(0), n_error(1), d_error);
 }
 
 /* ************************************************************************* */
 OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
-  Vector2 n_v(v(0), v(1));
+  return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));
-  Unit3 n_retracted = n_.retract(n_v);
-  double d_retracted = d_ + v(2);
-  return OrientedPlane3(n_retracted, d_retracted);
 }
 
 /* ************************************************************************* */
 Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
   Vector2 n_local = n_.localCoordinates(y.n_);
   double d_local = d_ - y.d_;
-  Vector3 e;
   return Vector3(n_local(0), n_local(1), -d_local);
 }
 

gtsam/geometry/OrientedPlane3.h

@@ -79,16 +79,29 @@ public:
   /// @}
 
   /** Transforms a plane to the specified pose
+   * @param xr a transformation in current coordiante
+   * @param Hp optional Jacobian wrpt the destination plane
+   * @param Hr optional jacobian wrpt the pose transformation
+   * @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 A transformation in current coordiante
+   * @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);
 
-  /** Computes the error between two poses.
+  /** Computes the error between two planes.
    *  The error is a norm 1 difference in tangent space.
    * @param the other plane
    */

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -48,36 +48,46 @@ TEST (OrientedPlane3, getMethods) {
   EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector()));
 }
 
+
 //*******************************************************************************
+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 transforming a plane to a pose
   gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0),
       gtsam::Point3(2.0, 3.0, 4.0));
   OrientedPlane3 plane(-1, 0, 0, 5);
-  OrientedPlane3 expected_meas(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
+  OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
-  OrientedPlane3 transformed_plane = OrientedPlane3::Transform(plane, pose,
+  OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
       none, none);
-  EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9));
+  OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
+  EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9));
+  EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-9));
 
   // Test the jacobians of transform
   Matrix actualH1, expectedH1, actualH2, expectedH2;
   {
-    expectedH1 = numericalDerivative11<OrientedPlane3, Pose3>(
+      OrientedPlane3::Transform(plane, pose, actualH1, none);
-        boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose);
+      // because the Transform class uses a wrong order of Jacobians in interface
-
+      expectedH1 = numericalDerivative22(Transform_, plane, pose);
-    OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1,
-        none);
       EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
-  }
+      OrientedPlane3::Transform(plane, pose, none, actualH2);
-  {
+      expectedH2 = numericalDerivative21(Transform_, plane, pose);
-    expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3>(
+      EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
-        boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane);
+  }
-
+  {
-    OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none,
+      plane.transform(pose, actualH1, none);
-        actualH2);
+      expectedH1 = numericalDerivative21(transform_, plane, pose);
+      EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
+      plane.transform(pose, none, actualH2);
+      expectedH2 = numericalDerivative22(Transform_, plane, pose);
       EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
   }
-
 }
 
 //*******************************************************************************