From 13be5add6e9ae205b57041c51156fe44c9a4df90 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 25 Jun 2015 02:11:28 -0400 Subject: [PATCH] change: add transform member class, and deprecate the static Transform class --- gtsam/geometry/OrientedPlane3.cpp | 38 ++++++++++++++---- gtsam/geometry/OrientedPlane3.h | 17 +++++++- gtsam/geometry/tests/testOrientedPlane3.cpp | 44 +++++++++++++-------- 3 files changed, 72 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 51d7a903b..22a1ad3aa 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/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_error); + return Vector3(n_error(0), n_error(1), d_ - plane.d_); } /* ************************************************************************* */ OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { - Vector2 n_v(v(0), v(1)); - Unit3 n_retracted = n_.retract(n_v); - double d_retracted = d_ + v(2); - return OrientedPlane3(n_retracted, d_retracted); + return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); } /* ************************************************************************* */ 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); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 55e7188af..ff57b590f 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/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 */ diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 127bc3d0c..9d48fc7d5 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/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 transformed_plane = OrientedPlane3::Transform(plane, pose, + OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); + 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( - boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose); - - OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1, - none); - EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + OrientedPlane3::Transform(plane, pose, actualH1, none); + // because the Transform class uses a wrong order of Jacobians in interface + expectedH1 = numericalDerivative22(Transform_, plane, pose); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + OrientedPlane3::Transform(plane, pose, none, actualH2); + expectedH2 = numericalDerivative21(Transform_, plane, pose); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } { - expectedH2 = numericalDerivative11( - boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane); - - OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none, - actualH2); - EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); + plane.transform(pose, actualH1, none); + 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)); } - } //*******************************************************************************