From 235cb532f4cfaf4a71287dcad853dc19f7504d84 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 12 Feb 2015 16:31:04 +0100 Subject: [PATCH] BORG-style comments --- gtsam/geometry/OrientedPlane3.cpp | 71 ++++++++++++++----------------- gtsam/geometry/OrientedPlane3.h | 48 ++++++++++----------- gtsam/geometry/Unit3.h | 44 ++++++++++--------- 3 files changed, 80 insertions(+), 83 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index e4d2092ea..088a84243 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -27,77 +27,72 @@ namespace gtsam { /* ************************************************************************* */ /// The print fuction void OrientedPlane3::print(const std::string& s) const { - Vector coeffs = planeCoefficients (); + Vector coeffs = planeCoefficients(); cout << s << " : " << coeffs << endl; } - /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane, - const gtsam::Pose3& xr, - OptionalJacobian<3, 6> Hr, - OptionalJacobian<3, 3> Hp) { +OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane, + const gtsam::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); + Unit3 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); + 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 (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 (); + 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; + *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); } - + /* ************************************************************************* */ -Vector3 OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const -{ - Vector2 n_error = -n_.localCoordinates (plane.n_); +Vector3 OrientedPlane3::error(const gtsam::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; + e << n_error(0), n_error(1), d_error; return (e); } /* ************************************************************************* */ OrientedPlane3 OrientedPlane3::retract(const Vector& 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); - return OrientedPlane3 (n_retracted, d_retracted); + 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); } /* ************************************************************************* */ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { - Vector n_local = n_.localCoordinates (y.n_); + Vector n_local = n_.localCoordinates(y.n_); double d_local = d_ - y.d_; Vector3 e; - e << n_local (0), n_local (1), -d_local; + e << n_local(0), n_local(1), -d_local; return e; } /* ************************************************************************* */ -Vector3 OrientedPlane3::planeCoefficients () const -{ - Vector unit_vec = n_.unitVector (); +Vector3 OrientedPlane3::planeCoefficients() const { + Vector unit_vec = n_.unitVector(); 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 f5bd0ec2e..28b67cb4e 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -35,32 +35,29 @@ private: double d_; /// The perpendicular distance to this plane public: - enum { dimension = 3 }; + enum { + dimension = 3 + }; /// @name Constructors /// @{ /// 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) - {} - - OrientedPlane3 (Vector vec) - : n_ (vec (0), vec (1), vec (2)), - d_ (vec (3)) - { + OrientedPlane3(const Unit3& 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); + Point3 p(a, b, c); n_ = Unit3(p); d_ = d; } @@ -70,17 +67,16 @@ public: /// 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)); + 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, - OptionalJacobian<3, 6> Hr = boost::none, - OptionalJacobian<3, 3> Hp = boost::none); + 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 - Vector3 error (const gtsam::OrientedPlane3& plane) const; + Vector3 error(const gtsam::OrientedPlane3& plane) const; /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { @@ -99,18 +95,22 @@ public: Vector3 localCoordinates(const OrientedPlane3& s) const; /// Returns the plane coefficients (a, b, c, d) - Vector3 planeCoefficients () const; + Vector3 planeCoefficients() const; - inline Unit3 normal () const { + inline Unit3 normal() const { return n_; } /// @} }; -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold< + OrientedPlane3> { +}; } // namespace gtsam diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 2bcd5dfa8..7d145c213 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -38,7 +38,9 @@ private: public: - enum { dimension = 2 }; + enum { + dimension = 2 + }; /// @name Constructors /// @{ @@ -65,8 +67,8 @@ public: } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = - boost::none); + static Unit3 FromPoint3(const Point3& point, // + OptionalJacobian<2, 3> H = boost::none); /// Random direction, using boost::uniform_on_sphere static Unit3 Random(boost::mt19937 & rng); @@ -99,7 +101,7 @@ public: Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(OptionalJacobian<3,2> H = boost::none) const { + const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const { if (H) *H = basis(); return p_; @@ -109,7 +111,7 @@ public: Vector unitVector(boost::optional H = boost::none) const { if (H) *H = basis(); - return (p_.vector ()); + return (p_.vector()); } /// Return scaled direction as Point3 @@ -118,12 +120,10 @@ public: } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, - OptionalJacobian<2,2> H = boost::none) const; + Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; /// Distance between two directions - double distance(const Unit3& q, - OptionalJacobian<1,2> H = boost::none) const; + double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; /// @} @@ -160,25 +160,27 @@ private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(p_); - // homebrew serialize Eigen Matrix - ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); - ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); - ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); - ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); - ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); - ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); - } + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(p_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0, 1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1, 0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1, 1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2, 0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2, 1)); + } /// @} }; // Define GTSAM traits -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold { +}; -template <> struct traits : public internal::Manifold {}; +template<> struct traits : public internal::Manifold { +}; } // namespace gtsam