diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp new file mode 100644 index 000000000..24ffae3ee --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.cpp + * @date Dec 19, 2013 + * @author Alex Trevor + * @brief A plane, represented by a normal direction and perpendicular distance + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +/// The print fuction +void OrientedPlane3::print(const std::string& s) const { + Vector coeffs = planeCoefficients (); + cout << s << " : " << coeffs << endl; +} + + +/* ************************************************************************* */ + OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, + boost::optional Hr, + boost::optional Hp) + { + Matrix n_hr; + Matrix n_hp; + Sphere2 n_rotated = xr.rotation ().unrotate (plane.n_, n_hr, n_hp); + + Vector n_unit = plane.n_.unitVector (); + Vector 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).block<2,3> (0,0) = n_hr; + (*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; + *Hp = gtsam::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); + } + +/* ************************************************************************* */ + Vector OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const + { + Vector n_error = -n_.localCoordinates (plane.n_, Sphere2::EXPMAP); + double d_error = d_ - plane.d_; + return (Vector (3) << n_error (0), n_error (1), -d_error); + } + +/* ************************************************************************* */ +OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { + // Retract the Sphere2 + Vector2 n_v (v (0), v (1)); + Sphere2 n_retracted = n_.retract (n_v, Sphere2::EXPMAP); + 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); + double d_local = d_ - y.d_; + return Vector (3) << n_local (0), n_local (1), -d_local; +} + +/* ************************************************************************* */ + Vector OrientedPlane3::planeCoefficients () const + { + Vector unit_vec = n_.unitVector (); + return Vector (4) << unit_vec[0], unit_vec[1], unit_vec[2], d_; + } + +/* ************************************************************************* */ + +} diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h new file mode 100644 index 000000000..b9c462250 --- /dev/null +++ b/gtsam/geometry/OrientedPlane3.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file OrientedPlane3.h + * @date Dec 19, 2013 + * @author Alex Trevor + * @author Frank Dellaert + * @brief An infinite plane, represented by a normal direction and perpendicular distance + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/// Represents an infinite plane in 3D. +class OrientedPlane3: public DerivedValue { + +private: + + Sphere2 n_; /// The direction of the planar normal + double d_; /// The perpendicular distance to this plane + +public: + + /// @name Constructors + /// @{ + + /// Default constructor + OrientedPlane3() : + n_(), + d_ (0.0){ + } + + /// Construct from a Sphere2 and a distance + OrientedPlane3 (const Sphere2& s, double d) + : n_ (s), + d_ (d) + {} + + OrientedPlane3 (Vector vec) + : n_ (vec (0), vec (1), vec (2)), + d_ (vec (3)) + { + } + + + /// Construct from a, b, c, d + OrientedPlane3(double a, double b, double c, double d) { + Point3 p (a, b, c); + n_ = Sphere2(p); + d_ = d; + } + + /// The print fuction + void print(const std::string& s = std::string()) const; + + /// The equals function with tolerance + bool equals(const OrientedPlane3& s, double tol = 1e-9) const { + return (n_.equals(s.n_, tol) && (fabs (d_ - s.d_) < tol)); + } + + /// Transforms a plane to the specified pose + static OrientedPlane3 Transform (const gtsam::OrientedPlane3& plane, + const gtsam::Pose3& xr, + boost::optional Hr, + boost::optional Hp); + + /// Computes the error between two poses + Vector error (const gtsam::OrientedPlane3& plane) const; + + /// Dimensionality of tangent space = 3 DOF + inline static size_t Dim() { + return 3; + } + + /// Dimensionality of tangent space = 3 DOF + inline size_t dim() const { + return 3; + } + + /// The retract function + OrientedPlane3 retract(const Vector& v) const; + + /// The local coordinates function + Vector localCoordinates(const OrientedPlane3& s) const; + + /// Returns the plane coefficients (a, b, c, d) + Vector planeCoefficients () const; + + /// @} +}; + +} // namespace gtsam + diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp new file mode 100644 index 000000000..cd0f5aaa7 --- /dev/null +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3 class + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +//******************************************************************************* +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, boost::none, boost::none); + EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9)); + + // Test the jacobians of transform + Matrix actualH1, expectedH1, actualH2, expectedH2; + { + expectedH1 = numericalDerivative11(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose); + + OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none); + EXPECT (assert_equal (expectedH1, actualH1, 1e-9)); + } + { + expectedH2 = numericalDerivative11 (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane); + + OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2); + EXPECT (assert_equal (expectedH2, actualH2, 1e-9)); + } + + } + +//******************************************************************************* +/// Returns a random vector -- copied from testSphere2.cpp +inline static Vector randomVector(const Vector& minLimits, + const Vector& maxLimits) { + + // Get the number of dimensions and create the return vector + size_t numDims = dim(minLimits); + Vector vector = zero(numDims); + + // Create the random vector + for (size_t i = 0; i < numDims; i++) { + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } + return vector; +} + +//******************************************************************************* +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); + for (size_t i = 0; i < numIterations; i++) { + + sleep(0); + + // Create a Plane + OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit)); + 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; + 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)); + OrientedPlane3 actual_p2 = p1.retract(actual_v12); + EXPECT(assert_equal(p2, actual_p2, 1e-3)); + } +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h new file mode 100644 index 000000000..db570e830 --- /dev/null +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -0,0 +1,63 @@ +/* + * @file OrientedPlane3Factor.cpp + * @brief OrientedPlane3 Factor class + * @author Alex Trevor + * @date December 22, 2013 + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Factor to measure a planar landmark from a given pose + */ + class OrientedPlane3Factor: public NoiseModelFactor2 { + + protected: + Symbol poseSymbol_; + Symbol landmarkSymbol_; + Vector3 measured_coeffs_; + OrientedPlane3 measured_p_; + + typedef NoiseModelFactor2 Base; + + public: + + /// Constructor + OrientedPlane3Factor () + {} + + /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol + OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel, + const Symbol& pose, + const Symbol& landmark) + : Base (noiseModel, pose, landmark), + poseSymbol_ (pose), + landmarkSymbol_ (landmark), + measured_coeffs_ (z) + { + measured_p_ = OrientedPlane3 (Sphere2 (z (0), z (1), z (2)), z (3)); + } + + /// print + void print(const std::string& s="PlaneFactor") const; + + virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2); + Vector error = predicted_plane.error (measured_p_); + return (error); + }; + }; + +} // gtsam +