From 6c34ce1e80729587ab7a08d39bb35d42c619d9bd Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 24 Jun 2015 16:11:29 -0400 Subject: [PATCH 01/13] change: add some comments --- gtsam/geometry/OrientedPlane3.h | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index d987ad7b3..661577739 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -27,12 +27,12 @@ namespace gtsam { /// Represents an infinite plane in 3D. -class OrientedPlane3: public DerivedValue { +class GTSAM_EXPORT OrientedPlane3 { private: - Unit3 n_; /// The direction of the planar normal - double d_; /// The perpendicular distance to this plane + Unit3 n_; ///< The direction of the planar normal + double d_; ///< The perpendicular distance to this plane public: enum { @@ -51,17 +51,22 @@ public: n_(s), d_(d) { } + /// Construct from a vector of plane coefficients OrientedPlane3(const Vector4& vec) : n_(vec(0), vec(1), vec(2)), d_(vec(3)) { } - /// Construct from a, b, c, d + /// 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); d_ = d; } + /// @} + /// @name Testable + /// @{ + /// The print function void print(const std::string& s = std::string()) const; @@ -70,12 +75,20 @@ public: return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); } - /// Transforms a plane to the specified pose + /** Transforms a plane to the specified pose + * @param The raw plane + * @param A transformation in current coordiante + * @param Hr optional jacobian wrpt incremental Pose + * @param Hp optional Jacobian wrpt the destination plane + */ static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, const gtsam::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 poses. + * The error is a norm 1 difference in tangent space. + * @param the other plane + */ Vector3 error(const gtsam::OrientedPlane3& plane) const; /// Dimensionality of tangent space = 3 DOF @@ -94,7 +107,7 @@ public: /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; - /// Returns the plane coefficients (a, b, c, d) + /// Returns the plane coefficients Vector3 planeCoefficients() const; inline Unit3 normal() const { From ff14e576ee27e0db81c61b716a6c3ef02d3b4984 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 24 Jun 2015 16:18:33 -0400 Subject: [PATCH 02/13] fix: return plane coefficents dimension was wrong, should be Vector4, not Vector3 --- gtsam/geometry/OrientedPlane3.cpp | 4 ++-- gtsam/geometry/OrientedPlane3.h | 2 +- gtsam/geometry/tests/testOrientedPlane3.cpp | 12 ++++++++++++ 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index e96708942..b6594c29c 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -91,9 +91,9 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { } /* ************************************************************************* */ -Vector3 OrientedPlane3::planeCoefficients() const { +Vector4 OrientedPlane3::planeCoefficients() const { Vector unit_vec = n_.unitVector(); - Vector3 a; + Vector4 a; a << unit_vec[0], unit_vec[1], unit_vec[2], d_; return a; } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 661577739..e75e32c96 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -108,7 +108,7 @@ public: Vector3 localCoordinates(const OrientedPlane3& s) const; /// Returns the plane coefficients - Vector3 planeCoefficients() const; + Vector4 planeCoefficients() const; inline Unit3 normal() const { return n_; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index b2b4ecc43..8a0c2f846 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -29,6 +29,18 @@ using boost::none; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) +//******************************************************************************* +TEST (OrientedPlane3, get) { + Vector4 c; + c << -1, 0, 0, 5; + OrientedPlane3 plane1(c); + OrientedPlane3 plane2(c[0], c[1], c[2], c[3]); + Vector coefficient1 = plane1.planeCoefficients(); + EXPECT(assert_equal(coefficient1, c, 1e-8)); + Vector coefficient2 = plane2.planeCoefficients(); + EXPECT(assert_equal(coefficient2, c, 1e-8)); +} + //******************************************************************************* TEST (OrientedPlane3, transform) { // Test transforming a plane to a pose From b7c1097f5811ceb16455bd90d16bd71c8c62e25a Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 24 Jun 2015 16:29:38 -0400 Subject: [PATCH 03/13] feature: add inline get methods --- gtsam/geometry/OrientedPlane3.cpp | 10 ---------- gtsam/geometry/OrientedPlane3.h | 11 ++++++++++- gtsam/geometry/tests/testOrientedPlane3.cpp | 8 ++++++-- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index b6594c29c..dff4eac9e 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -90,14 +90,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { return e; } -/* ************************************************************************* */ -Vector4 OrientedPlane3::planeCoefficients() const { - Vector unit_vec = n_.unitVector(); - Vector4 a; - a << unit_vec[0], unit_vec[1], unit_vec[2], d_; - return a; -} - -/* ************************************************************************* */ - } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index e75e32c96..8ff29b88a 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -108,12 +108,21 @@ public: Vector3 localCoordinates(const OrientedPlane3& s) const; /// Returns the plane coefficients - Vector4 planeCoefficients() const; + inline Vector4 planeCoefficients() const { + Vector3 unit_vec = n_.unitVector(); + return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_); + } + /// Return the normal inline Unit3 normal() const { return n_; } + /// Return the perpendicular distance to the origin + inline double distance() const { + return d_; + } + /// @} }; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 8a0c2f846..c6189b970 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -35,10 +35,14 @@ TEST (OrientedPlane3, get) { c << -1, 0, 0, 5; OrientedPlane3 plane1(c); OrientedPlane3 plane2(c[0], c[1], c[2], c[3]); - Vector coefficient1 = plane1.planeCoefficients(); + Vector4 coefficient1 = plane1.planeCoefficients(); + double distance1 = plane1.distance(); EXPECT(assert_equal(coefficient1, c, 1e-8)); - Vector coefficient2 = plane2.planeCoefficients(); + EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); + Vector4 coefficient2 = plane2.planeCoefficients(); + double distance2 = plane2.distance(); EXPECT(assert_equal(coefficient2, c, 1e-8)); + EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8); } //******************************************************************************* From cd0fe5d98c38a344f195881f96b3cbd950a00814 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 24 Jun 2015 22:15:25 -0400 Subject: [PATCH 04/13] change: correct the naming, and inappropriate tests --- gtsam/geometry/tests/testOrientedPlane3.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index c6189b970..1fd9d9d6b 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -30,7 +30,7 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) //******************************************************************************* -TEST (OrientedPlane3, get) { +TEST (OrientedPlane3, getMethods) { Vector4 c; c << -1, 0, 0, 5; OrientedPlane3 plane1(c); @@ -38,11 +38,13 @@ TEST (OrientedPlane3, get) { Vector4 coefficient1 = plane1.planeCoefficients(); double distance1 = plane1.distance(); EXPECT(assert_equal(coefficient1, c, 1e-8)); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane1.normal().unitVector())); EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); Vector4 coefficient2 = plane2.planeCoefficients(); double distance2 = plane2.distance(); EXPECT(assert_equal(coefficient2, c, 1e-8)); EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector())); } //******************************************************************************* @@ -98,11 +100,11 @@ inline static Vector randomVector(const Vector& minLimits, TEST(OrientedPlane3, localCoordinates_retract) { size_t numIterations = 10000; - gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); + Vector4 minPlaneLimit, maxPlaneLimit; minPlaneLimit << -1.0, -1.0, -1.0, 0.01; maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; - Vector minXiLimit(3), maxXiLimit(3); + Vector3 minXiLimit, maxXiLimit; minXiLimit << -M_PI, -M_PI, -10.0; maxXiLimit << M_PI, M_PI, 10.0; for (size_t i = 0; i < numIterations; i++) { @@ -114,15 +116,15 @@ TEST(OrientedPlane3, localCoordinates_retract) { Vector v12 = randomVector(minXiLimit, maxXiLimit); // Magnitude of the rotation can be at most pi - if (v12.segment<3>(0).norm() > M_PI) - v12.segment<3>(0) = v12.segment<3>(0) / M_PI; + if (v12.head<3>().norm() > M_PI) + v12.head<3>() = v12.head<3>() / M_PI; OrientedPlane3 p2 = p1.retract(v12); // Check if the local coordinates and retract return the same results. Vector actual_v12 = p1.localCoordinates(p2); - EXPECT(assert_equal(v12, actual_v12, 1e-3)); + EXPECT(assert_equal(v12, actual_v12, 1e-6)); OrientedPlane3 actual_p2 = p1.retract(actual_v12); - EXPECT(assert_equal(p2, actual_p2, 1e-3)); + EXPECT(assert_equal(p2, actual_p2, 1e-6)); } } From 2c1d8e38db65a0f80bde6e068ff1ab852cf48213 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 24 Jun 2015 22:24:09 -0400 Subject: [PATCH 05/13] change: remove redudant namespace --- gtsam/geometry/OrientedPlane3.cpp | 18 +++++++++--------- gtsam/geometry/OrientedPlane3.h | 7 +++---- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index dff4eac9e..48bd07382 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -26,27 +26,27 @@ namespace gtsam { /* ************************************************************************* */ /// The print fuction -void OrientedPlane3::print(const std::string& s) const { - Vector coeffs = planeCoefficients(); +void OrientedPlane3::print(const string& s) const { + Vector4 coeffs = planeCoefficients(); cout << s << " : " << coeffs << endl; } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, - const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr, +OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, + const Pose3& xr, OptionalJacobian<3, 6> Hr, OptionalJacobian<3, 3> Hp) { Matrix n_hr; Matrix n_hp; Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); - Vector n_unit = plane.n_.unitVector(); - Vector unit_vec = n_rotated.unitVector(); + Vector3 n_unit = plane.n_.unitVector(); + Vector3 unit_vec = n_rotated.unitVector(); double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); if (Hr) { - *Hr = gtsam::zeros(3, 6); + *Hr = zeros(3, 6); (*Hr).block<2, 3>(0, 0) = n_hr; (*Hr).block<1, 3>(2, 3) = unit_vec; } @@ -54,7 +54,7 @@ OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, Vector xrp = xr.translation().vector(); Matrix n_basis = plane.n_.basis(); Vector hpp = n_basis.transpose() * xrp; - *Hp = gtsam::zeros(3, 3); + *Hp = zeros(3, 3); (*Hp).block<2, 2>(0, 0) = n_hp; (*Hp).block<1, 2>(2, 0) = hpp; (*Hp)(2, 2) = 1; @@ -64,7 +64,7 @@ OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, } /* ************************************************************************* */ -Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { +Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { Vector2 n_error = -n_.localCoordinates(plane.n_); double d_error = d_ - plane.d_; Vector3 e; diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 8ff29b88a..a6fee420b 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -22,7 +22,6 @@ #include #include #include -#include namespace gtsam { @@ -81,15 +80,15 @@ public: * @param Hr optional jacobian wrpt incremental Pose * @param Hp optional Jacobian wrpt the destination plane */ - static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, - const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, + 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. * The error is a norm 1 difference in tangent space. * @param the other plane */ - Vector3 error(const gtsam::OrientedPlane3& plane) const; + Vector3 error(const OrientedPlane3& plane) const; /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { From 89eec718dad11cf15ebcc7b471766ab2eb4f8080 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Wed, 24 Jun 2015 22:34:38 -0400 Subject: [PATCH 06/13] change: matrix with fixed size in OrientedPlane3 --- gtsam/geometry/OrientedPlane3.cpp | 14 ++++++-------- gtsam/geometry/Unit3.h | 2 +- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 48bd07382..f40ad1c49 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -35,15 +35,13 @@ void OrientedPlane3::print(const string& s) const { OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, const Pose3& xr, OptionalJacobian<3, 6> Hr, OptionalJacobian<3, 3> Hp) { - Matrix n_hr; - Matrix n_hp; + Matrix23 n_hr; + Matrix22 n_hp; Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); Vector3 n_unit = plane.n_.unitVector(); Vector3 unit_vec = n_rotated.unitVector(); double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; - OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2), - pred_d); if (Hr) { *Hr = zeros(3, 6); @@ -51,16 +49,16 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, (*Hr).block<1, 3>(2, 3) = unit_vec; } if (Hp) { - Vector xrp = xr.translation().vector(); - Matrix n_basis = plane.n_.basis(); - Vector hpp = n_basis.transpose() * xrp; + Vector3 xrp = xr.translation().vector(); + Matrix32 n_basis = plane.n_.basis(); + Vector2 hpp = n_basis.transpose() * xrp; *Hp = zeros(3, 3); (*Hp).block<2, 2>(0, 0) = n_hp; (*Hp).block<1, 2>(2, 0) = hpp; (*Hp)(2, 2) = 1; } - return (transformed_plane); + return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 12bfac5ce..f8cea092e 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -22,7 +22,7 @@ #include #include -#include + #include #include From 68daf2e9846ec51410a3e211ebf4fe133795d9cb Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 25 Jun 2015 01:27:20 -0400 Subject: [PATCH 07/13] change: clean the DerivedValue headers in other files --- gtsam/geometry/Cal3Bundler.h | 1 - gtsam/geometry/Cal3DS2.h | 1 - gtsam/geometry/Cal3Unified.h | 1 - 3 files changed, 3 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 234ee261a..d95c47f7b 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 9f4641f71..81463ac06 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 99bd04fb1..9982cec47 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -23,7 +23,6 @@ #pragma once #include -#include namespace gtsam { From a740acfeceead2b2ec8d12ed39e4eb8d2b7f9621 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 25 Jun 2015 01:27:58 -0400 Subject: [PATCH 08/13] change: clean redundant temporary variables --- gtsam/geometry/OrientedPlane3.cpp | 25 ++++++++------------- gtsam/geometry/OrientedPlane3.h | 6 ++++- gtsam/geometry/tests/testOrientedPlane3.cpp | 1 + 3 files changed, 15 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index f40ad1c49..51d7a903b 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -13,6 +13,7 @@ * @file OrientedPlane3.cpp * @date Dec 19, 2013 * @author Alex Trevor + * @author Zhaoyang Lv * @brief A plane, represented by a normal direction and perpendicular distance */ @@ -25,7 +26,6 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -/// The print fuction void OrientedPlane3::print(const string& s) const { Vector4 coeffs = planeCoefficients(); cout << s << " : " << coeffs << endl; @@ -39,22 +39,19 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, Matrix22 n_hp; Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); - Vector3 n_unit = plane.n_.unitVector(); Vector3 unit_vec = n_rotated.unitVector(); - double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_; + double pred_d = plane.n_.unitVector().dot(xr.translation().vector()) + plane.d_; if (Hr) { *Hr = zeros(3, 6); - (*Hr).block<2, 3>(0, 0) = n_hr; - (*Hr).block<1, 3>(2, 3) = unit_vec; + Hr->block<2, 3>(0, 0) = n_hr; + Hr->block<1, 3>(2, 3) = unit_vec; } if (Hp) { - Vector3 xrp = xr.translation().vector(); - Matrix32 n_basis = plane.n_.basis(); - Vector2 hpp = n_basis.transpose() * xrp; + Vector2 hpp = plane.n_.basis().transpose() * xr.translation().vector(); *Hp = zeros(3, 3); - (*Hp).block<2, 2>(0, 0) = n_hp; - (*Hp).block<1, 2>(2, 0) = hpp; + Hp->block<2, 2>(0, 0) = n_hp; + Hp->block<1, 2>(2, 0) = hpp; (*Hp)(2, 2) = 1; } @@ -65,14 +62,11 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { Vector2 n_error = -n_.localCoordinates(plane.n_); double d_error = d_ - plane.d_; - Vector3 e; - e << n_error(0), n_error(1), d_error; - return (e); + return Vector3(n_error(0), n_error(1), d_error); } /* ************************************************************************* */ OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { - // Retract the Unit3 Vector2 n_v(v(0), v(1)); Unit3 n_retracted = n_.retract(n_v); double d_retracted = d_ + v(2); @@ -84,8 +78,7 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { Vector2 n_local = n_.localCoordinates(y.n_); double d_local = d_ - y.d_; Vector3 e; - e << n_local(0), n_local(1), -d_local; - return e; + return Vector3(n_local(0), n_local(1), -d_local); } } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index a6fee420b..55e7188af 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -14,6 +14,7 @@ * @date Dec 19, 2013 * @author Alex Trevor * @author Frank Dellaert + * @author Zhaoyang Lv * @brief An infinite plane, represented by a normal direction and perpendicular distance */ @@ -37,6 +38,7 @@ public: enum { dimension = 3 }; + /// @name Constructors /// @{ @@ -74,10 +76,12 @@ public: return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); } + /// @} + /** Transforms a plane to the specified pose * @param The raw plane * @param A transformation in current coordiante - * @param Hr optional jacobian wrpt incremental Pose + * @param Hr optional jacobian wrpt the pose transformation * @param Hp optional Jacobian wrpt the destination plane */ static OrientedPlane3 Transform(const OrientedPlane3& plane, diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 1fd9d9d6b..127bc3d0c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -13,6 +13,7 @@ * @file testOrientedPlane3.cpp * @date Dec 19, 2012 * @author Alex Trevor + * @author Zhaoyang Lv * @brief Tests the OrientedPlane3 class */ From 13be5add6e9ae205b57041c51156fe44c9a4df90 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 25 Jun 2015 02:11:28 -0400 Subject: [PATCH 09/13] 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)); } - } //******************************************************************************* From d8f0a35a9a048bb7d16cf57803e0baf7ab848a0a Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 25 Jun 2015 02:14:06 -0400 Subject: [PATCH 10/13] change: derivative naming changs according to our convention --- gtsam/geometry/OrientedPlane3.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 22a1ad3aa..f9f30970a 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -35,22 +35,22 @@ void OrientedPlane3::print(const string& s) const { OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, const Pose3& xr, OptionalJacobian<3, 6> Hr, OptionalJacobian<3, 3> Hp) { - Matrix23 n_hr; - Matrix22 n_hp; - Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp); + Matrix23 D_rotated_plane; + Matrix22 D_rotated_pose; + Unit3 n_rotated = xr.rotation().unrotate(plane.n_, D_rotated_plane, D_rotated_pose); Vector3 unit_vec = n_rotated.unitVector(); double pred_d = plane.n_.unitVector().dot(xr.translation().vector()) + plane.d_; if (Hr) { *Hr = zeros(3, 6); - Hr->block<2, 3>(0, 0) = n_hr; + Hr->block<2, 3>(0, 0) = D_rotated_plane; Hr->block<1, 3>(2, 3) = unit_vec; } if (Hp) { Vector2 hpp = plane.n_.basis().transpose() * xr.translation().vector(); *Hp = Z_3x3; - Hp->block<2, 2>(0, 0) = n_hp; + Hp->block<2, 2>(0, 0) = D_rotated_pose; Hp->block<1, 2>(2, 0) = hpp; (*Hp)(2, 2) = 1; } @@ -61,22 +61,22 @@ OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, /* ************************************************************************* */ 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); + Matrix23 D_rotated_plane; + Matrix22 D_rotated_pose; + Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); 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<2, 3>(0, 0) = D_rotated_plane; 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<2, 2>(0, 0) = D_rotated_pose; Hp->block<1, 2>(2, 0) = hpp; (*Hp)(2, 2) = 1; } From e45f5faea1734abc4c30198136d735138e259b30 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Thu, 25 Jun 2015 09:44:18 -0400 Subject: [PATCH 11/13] change: formatting --- gtsam/geometry/OrientedPlane3.h | 14 ++++----- gtsam/geometry/tests/testOrientedPlane3.cpp | 32 ++++++++++----------- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index ff57b590f..2d2527a25 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -44,17 +44,17 @@ public: /// Default constructor OrientedPlane3() : - n_(), d_(0.0) { + n_(), d_(0.0) { } /// Construct from a Unit3 and a distance OrientedPlane3(const Unit3& s, double d) : - n_(s), d_(d) { + n_(s), d_(d) { } /// Construct from a vector of plane coefficients OrientedPlane3(const Vector4& vec) : - n_(vec(0), vec(1), vec(2)), d_(vec(3)) { + n_(vec(0), vec(1), vec(2)), d_(vec(3)) { } /// Construct from four numbers of plane coeffcients (a, b, c, d) @@ -85,8 +85,8 @@ public: * @return the transformed plane */ OrientedPlane3 transform(const Pose3& xr, - OptionalJacobian<3, 3> Hp = boost::none, - OptionalJacobian<3, 6> Hr = boost::none) const; + OptionalJacobian<3, 3> Hp = boost::none, + OptionalJacobian<3, 6> Hr = boost::none) const; /** * @ deprecated the static method has wrong Jacobian order, @@ -143,11 +143,11 @@ public: }; template<> struct traits : public internal::Manifold< - OrientedPlane3> { +OrientedPlane3> { }; template<> struct traits : public internal::Manifold< - OrientedPlane3> { +OrientedPlane3> { }; } // namespace gtsam diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 9d48fc7d5..11931449f 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -51,11 +51,11 @@ TEST (OrientedPlane3, getMethods) { //******************************************************************************* OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) { - return OrientedPlane3::Transform(plane, xr); + return OrientedPlane3::Transform(plane, xr); } OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { - return plane.transform(xr); + return plane.transform(xr); } TEST (OrientedPlane3, transform) { @@ -72,26 +72,26 @@ TEST (OrientedPlane3, transform) { // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; { - 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)); + // 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-9)); + OrientedPlane3::Transform(plane, pose, none, actualH2); + expectedH2 = numericalDerivative21(Transform_, plane, pose); + 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)); + 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)); } } //******************************************************************************* -// Returns a random vector -- copied from testUnit3.cpp +// Returns a any size random vector inline static Vector randomVector(const Vector& minLimits, const Vector& maxLimits) { From 7ebc9e48e515b3d10dea51d616bd1625363312fa Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 25 Jun 2015 11:31:47 -0400 Subject: [PATCH 12/13] change: static method now calls the member method. Remove duplicate codes --- gtsam/geometry/OrientedPlane3.cpp | 27 --------------------------- gtsam/geometry/OrientedPlane3.h | 4 +++- 2 files changed, 3 insertions(+), 28 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index f9f30970a..95290497d 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -31,33 +31,6 @@ void OrientedPlane3::print(const string& s) const { cout << s << " : " << coeffs << endl; } -/* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::Transform(const OrientedPlane3& plane, - const Pose3& xr, OptionalJacobian<3, 6> Hr, - OptionalJacobian<3, 3> Hp) { - Matrix23 D_rotated_plane; - Matrix22 D_rotated_pose; - Unit3 n_rotated = xr.rotation().unrotate(plane.n_, D_rotated_plane, D_rotated_pose); - - Vector3 unit_vec = n_rotated.unitVector(); - double pred_d = plane.n_.unitVector().dot(xr.translation().vector()) + plane.d_; - - if (Hr) { - *Hr = zeros(3, 6); - Hr->block<2, 3>(0, 0) = D_rotated_plane; - Hr->block<1, 3>(2, 3) = unit_vec; - } - if (Hp) { - Vector2 hpp = plane.n_.basis().transpose() * xr.translation().vector(); - *Hp = Z_3x3; - Hp->block<2, 2>(0, 0) = D_rotated_pose; - Hp->block<1, 2>(2, 0) = hpp; - (*Hp)(2, 2) = 1; - } - - 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 { diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 2d2527a25..949e4a285 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -99,7 +99,9 @@ public: */ static OrientedPlane3 Transform(const OrientedPlane3& plane, const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, - OptionalJacobian<3, 3> Hp = 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. From b569a4ab9230239f3c7738ad53f75d5e771ed077 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 25 Jun 2015 11:41:58 -0400 Subject: [PATCH 13/13] feature: add some comment descriptions to the class --- gtsam/geometry/OrientedPlane3.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 949e4a285..5c9a5cdef 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -26,7 +26,12 @@ namespace gtsam { -/// Represents an infinite plane in 3D. +/** + * @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: @@ -89,7 +94,7 @@ public: OptionalJacobian<3, 6> Hr = boost::none) const; /** - * @ deprecated the static method has wrong Jacobian order, + * @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