diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index cfc9a183b..96616e8cc 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -40,7 +40,7 @@ void OrientedPlane3::print(const std::string& s) const { { Matrix n_hr; Matrix n_hp; - Sphere2 n_rotated = xr.rotation ().unrotate (plane.n_, n_hr, 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 (); @@ -68,34 +68,40 @@ void OrientedPlane3::print(const std::string& s) const { } /* ************************************************************************* */ - Vector OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const + Vector3 OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const { - Vector n_error = -n_.localCoordinates (plane.n_, Sphere2::EXPMAP); + Vector2 n_error = -n_.localCoordinates (plane.n_); double d_error = d_ - plane.d_; - return (Vector (3) << n_error (0), n_error (1), d_error); + Vector3 e; + e << n_error (0), n_error (1), d_error; + return (e); } /* ************************************************************************* */ OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { - // Retract the Sphere2 + // Retract the Unit3 Vector2 n_v (v (0), v (1)); - Sphere2 n_retracted = n_.retract (n_v, Sphere2::EXPMAP); + Unit3 n_retracted = n_.retract (n_v); double d_retracted = d_ + v (2); return OrientedPlane3 (n_retracted, d_retracted); } /* ************************************************************************* */ -Vector OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { - Vector n_local = n_.localCoordinates (y.n_, Sphere2::EXPMAP); +Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { + Vector n_local = n_.localCoordinates (y.n_); double d_local = d_ - y.d_; - return Vector (3) << n_local (0), n_local (1), -d_local; + Vector3 e; + e << n_local (0), n_local (1), -d_local; + return e; } /* ************************************************************************* */ - Vector OrientedPlane3::planeCoefficients () const + Vector3 OrientedPlane3::planeCoefficients () const { Vector unit_vec = n_.unitVector (); - return Vector (4) << unit_vec[0], unit_vec[1], unit_vec[2], d_; + Vector3 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 0f0f20fe5..c73274dff 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -20,7 +20,7 @@ #pragma once #include -#include +#include #include #include @@ -31,11 +31,11 @@ class OrientedPlane3: public DerivedValue { private: - Sphere2 n_; /// The direction of the planar normal + Unit3 n_; /// The direction of the planar normal double d_; /// The perpendicular distance to this plane public: - + enum { dimension = 3 }; /// @name Constructors /// @{ @@ -45,8 +45,8 @@ public: d_ (0.0){ } - /// Construct from a Sphere2 and a distance - OrientedPlane3 (const Sphere2& s, double d) + /// Construct from a Unit3 and a distance + OrientedPlane3 (const Unit3& s, double d) : n_ (s), d_ (d) {} @@ -61,7 +61,7 @@ public: /// Construct from a, b, c, d OrientedPlane3(double a, double b, double c, double d) { Point3 p (a, b, c); - n_ = Sphere2(p); + n_ = Unit3(p); d_ = d; } @@ -80,7 +80,7 @@ public: boost::optional Hp); /// Computes the error between two poses - Vector error (const gtsam::OrientedPlane3& plane) const; + Vector3 error (const gtsam::OrientedPlane3& plane) const; /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { @@ -96,17 +96,21 @@ public: OrientedPlane3 retract(const Vector& v) const; /// The local coordinates function - Vector localCoordinates(const OrientedPlane3& s) const; + Vector3 localCoordinates(const OrientedPlane3& s) const; /// Returns the plane coefficients (a, b, c, d) - Vector planeCoefficients () const; + Vector3 planeCoefficients () const; - inline Sphere2 normal () const { + inline Unit3 normal () const { return n_; } /// @} }; +template <> struct traits : public internal::Manifold {}; + +template <> struct traits : public internal::Manifold {}; + } // namespace gtsam diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index cd0f5aaa7..122e91684 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -16,7 +16,7 @@ * @brief Tests the OrientedPlane3 class */ -#include +#include #include #include #include @@ -68,7 +68,7 @@ TEST (OrientedPlane3, transform) } //******************************************************************************* -/// Returns a random vector -- copied from testSphere2.cpp +/// Returns a random vector -- copied from testUnit3.cpp inline static Vector randomVector(const Vector& minLimits, const Vector& maxLimits) { @@ -88,9 +88,13 @@ inline static Vector randomVector(const Vector& minLimits, TEST(OrientedPlane3, localCoordinates_retract) { size_t numIterations = 10000; - Vector minPlaneLimit = Vector_(4, -1.0, -1.0, -1.0, 0.01), maxPlaneLimit = - Vector_(4, 1.0, 1.0, 10.0); - Vector minXiLimit = Vector_(3, -M_PI, -M_PI, -10.0), maxXiLimit = Vector_(3, M_PI, M_PI, 10.0); + Vector minPlaneLimit, maxPlaneLimit; + minPlaneLimit << 4, -1.0, -1.0, -1.0, 0.01; + maxPlaneLimit << 4, 1.0, 1.0, 10.0; + + Vector minXiLimit,maxXiLimit; + minXiLimit << -M_PI, -M_PI, -10.0; + maxXiLimit << M_PI, M_PI, 10.0; for (size_t i = 0; i < numIterations; i++) { sleep(0);