From 8e364cb34ee5002595ad9fdcc66b41dc2dae169e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 19 Aug 2010 20:03:06 +0000 Subject: [PATCH] Improvements to the Lie objects, with new member/static functions in most of the geometry objects. Many of the functions that were previously global have been moved to static functions. See Lie.h for more details. --- base/Lie-inl.h | 8 +-- base/Lie.h | 92 ++++++++++++++++++++++++----- geometry/Point2.h | 39 ++++++------ geometry/Point3.cpp | 2 + geometry/Point3.h | 42 ++++++------- geometry/Pose2.cpp | 34 ++++++----- geometry/Pose2.h | 53 ++++++++++------- geometry/Pose3.cpp | 10 ++-- geometry/Pose3.h | 60 +++++++++---------- geometry/Rot2.cpp | 4 +- geometry/Rot2.h | 81 +++++++++++++------------ geometry/Rot3.cpp | 60 +++++++++---------- geometry/Rot3.h | 101 ++++++++++++++++---------------- geometry/StereoPoint2.h | 48 +++++++-------- geometry/tests/testPose3.cpp | 46 ++++++++------- geometry/tests/testRot2.cpp | 8 +-- geometry/tests/testRot3.cpp | 42 ++++++------- geometry/timeRot3.cpp | 4 +- nonlinear/LieConfig-inl.h | 3 +- nonlinear/NonlinearConstraint.h | 8 +-- slam/BetweenConstraint.h | 2 +- slam/tests/testPose3Factor.cpp | 4 +- slam/tests/testSimulated3D.cpp | 10 ++-- 23 files changed, 422 insertions(+), 339 deletions(-) diff --git a/base/Lie-inl.h b/base/Lie-inl.h index 19aa09d153..5e4c39d08b 100644 --- a/base/Lie-inl.h +++ b/base/Lie-inl.h @@ -18,10 +18,10 @@ template class Lie; namespace gtsam { - template - size_t Lie::dim() const { - return gtsam::dim(*((T*)this)); - } +// template +// size_t Lie::dim() const { +// return gtsam::dim(*((T*)this)); +// } /** * Returns Exponential mapy diff --git a/base/Lie.h b/base/Lie.h index f36c298402..487f974603 100644 --- a/base/Lie.h +++ b/base/Lie.h @@ -1,37 +1,55 @@ -/* - * Lie.h - * - * Created on: Jan 5, 2010 - * Author: Richard Roberts +/** + * @file Lie.h + * @brief Base class and basic functions for Lie types + * @author Richard Roberts + * @author Alex Cunningham */ #pragma once #include + #include namespace gtsam { + /** + * These core global functions can be specialized by new Lie types + * for better performance. + */ + + /* Exponential map about identity */ template - T expmap(const Vector& v); /* Exponential map about identity */ + T expmap(const Vector& v) { return T::Expmap(v); } - // The following functions may be overridden in your own class file - // with more efficient versions if possible. + /* Logmap (inverse exponential map) about identity */ + template + Vector logmap(const T& p) { return T::Logmap(p); } - // Compute l1 s.t. l2=l1*l0 + /** Compute l1 s.t. l2=l1*l0 */ template inline T between(const T& l1, const T& l2) { return compose(inverse(l1),l2); } - // Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp + /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ template inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); } - /* Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ + /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ template inline T expmap(const T& t, const Vector& d) { return compose(t,expmap(d)); } /** * Base class for Lie group type + * This class uses the Curiously Recurring Template design pattern to allow + * for static polymorphism. + * + * T is the derived Lie type, like Point2, Pose3, etc. + * + * By convention, we use capital letters to designate a static function + * + * FIXME: Need to find a way to check for actual implementations in T + * so that there are no recursive function calls. This could be handled + * by not using the same name */ template class Lie { @@ -40,23 +58,65 @@ namespace gtsam { /** * Returns dimensionality of the tangent space */ - size_t dim() const; + inline size_t dim() const { + return static_cast(this)->dim(); + } /** - * Returns Exponential mapy + * Returns Exponential map update of T + * Default implementation calls global binary function */ T expmap(const Vector& v) const; + /** expmap around identity */ + static T Expmap(const Vector& v) { + return T::Expmap(v); + } + /** * Returns Log map + * Default Implementation calls global binary function */ Vector logmap(const T& lp) const; + /** Logmap around identity */ + static Vector Logmap(const T& p) { + return T::Logmap(p); + } + + /** compose with another object */ + inline T compose(const T& p) const { + return static_cast(this)->compose(p); + } + + /** invert the object and yield a new one */ + inline T inverse() const { + return static_cast(this)->inverse(); + } + }; + /** get the dimension of an object with a global function */ + template + inline size_t dim(const T& object) { + return object.dim(); + } + + /** compose two Lie types */ + template + inline T compose(const T& p1, const T& p2) { + return p1.compose(p2); + } + + /** invert an object */ + template + inline T inverse(const T& p) { + return p.inverse(); + } + /** Call print on the object */ template - inline void print_(const T& object, const std::string& s = "") { + inline void print(const T& object, const std::string& s = "") { object.print(s); } @@ -100,11 +160,11 @@ namespace gtsam { inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;} /** - * Three term approximation of the BakerÐCampbellÐHausdorff formula + * Three term approximation of the Baker�Campbell�Hausdorff formula * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 - * http://en.wikipedia.org/wiki/BakerÐCampbellÐHausdorff_formula + * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula */ template T BCH(const T& X, const T& Y) { diff --git a/geometry/Point2.h b/geometry/Point2.h index 2b472a3790..074b8e8fcb 100644 --- a/geometry/Point2.h +++ b/geometry/Point2.h @@ -22,8 +22,7 @@ namespace gtsam { class Point2: Testable, public Lie { public: /// dimension of the variable - used to autodetect sizes - static inline size_t dim() {return 2;} - + static const size_t dimension = 2; private: double x_, y_; @@ -33,12 +32,32 @@ namespace gtsam { Point2(double x, double y): x_(x), y_(y) {} Point2(const Vector& v) : x_(v(0)), y_(v(1)) {} + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + /** print with optional string */ void print(const std::string& s = "") const; /** equals with an tolerance, prints out message if unequal*/ bool equals(const Point2& q, double tol = 1e-9) const; + /** Lie requirements */ + + /** Size of the tangent space of the Lie type */ + inline size_t dim() const { return dimension; } + + /** "Compose", just adds the coordinates of two points. */ + Point2 compose(const Point2& p1) const { return *this+p1; } + + /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ + Point2 inverse() const { return Point2(-x_, -y_); } + + /** Exponential map around identity - just create a Point2 from a vector */ + static inline Point2 Expmap(const Vector& v) { return Point2(v); } + + /** Log map around identity - just return the Point2 as a vector */ + static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + /** get functions for x, y */ double x() const {return x_;} double y() const {return y_;} @@ -75,20 +94,7 @@ namespace gtsam { /** Lie group functions */ - /** Global print calls member function */ - inline void print(const Point2& p, const std::string& s = "Point2") { p.print(s); } - - /** Dimensionality of the tangent space */ - inline size_t dim(const Point2& obj) { return 2; } - - /** Exponential map around identity - just create a Point2 from a vector */ - template<> inline Point2 expmap(const Vector& dp) { return Point2(dp); } - - /** Log map around identity - just return the Point2 as a vector */ - inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } - /** "Compose", just adds the coordinates of two points. */ - inline Point2 compose(const Point2& p1, const Point2& p2) { return p1+p2; } inline Point2 compose(const Point2& p1, const Point2& p2, boost::optional H1, boost::optional H2=boost::none) { if(H1) *H1 = eye(2); @@ -113,9 +119,6 @@ namespace gtsam { return between(p1, p2); } - /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ - inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); } - /** multiply with scalar */ inline Point2 operator*(double s, const Point2& p) {return p*s;} } diff --git a/geometry/Point3.cpp b/geometry/Point3.cpp index 6a2efa1c8b..644a66ac8d 100644 --- a/geometry/Point3.cpp +++ b/geometry/Point3.cpp @@ -17,6 +17,8 @@ namespace gtsam { return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); } + /* ************************************************************************* */ + void Point3::print(const std::string& s) const { std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; } diff --git a/geometry/Point3.h b/geometry/Point3.h index 763042a807..eab52ce5a9 100644 --- a/geometry/Point3.h +++ b/geometry/Point3.h @@ -22,7 +22,8 @@ namespace gtsam { class Point3: Testable, public Lie { public: /// dimension of the variable - used to autodetect sizes - static inline size_t dim() {return 3;} + static const size_t dimension = 3; + private: double x_, y_, z_; @@ -38,6 +39,26 @@ namespace gtsam { /** equals with an tolerance */ bool equals(const Point3& p, double tol = 1e-9) const; + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /** Lie requirements */ + + /** return DOF, dimensionality of tangent space */ + inline size_t dim() const { return dimension; } + + /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ + inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } + + /** "Compose" - just adds coordinates of two points */ + inline Point3 compose(const Point3& p1) const { return *this+p1; } + + /** Exponential map at identity - just create a Point3 from x,y,z */ + static inline Point3 Expmap(const Vector& v) { return Point3(v); } + + /** Log map at identity - return the x,y,z of this point */ + static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } + /** return vectorized form (column-wise)*/ Vector vector() const { //double r[] = { x_, y_, z_ }; @@ -80,22 +101,7 @@ namespace gtsam { } }; - - /** Global print calls member function */ - inline void print(const Point3& p, const std::string& s) { p.print(s); } - inline void print(const Point3& p) { p.print(); } - - /** return DOF, dimensionality of tangent space */ - inline size_t dim(const Point3&) { return 3; } - - /** Exponential map at identity - just create a Point3 from x,y,z */ - template<> inline Point3 expmap(const Vector& dp) { return Point3(dp); } - - /** Log map at identity - return the x,y,z of this point */ - inline Vector logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } - /** "Compose" - just adds coordinates of two points */ - inline Point3 compose(const Point3& p1, const Point3& p0) { return p0+p1; } inline Matrix Dcompose1(const Point3& p1, const Point3& p0) { return Matrix_(3,3, 1.0, 0.0, 0.0, @@ -109,10 +115,6 @@ namespace gtsam { 0.0, 0.0, 1.0); } - /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ - inline Point3 inverse(const Point3& p) { return Point3(-p.x(), -p.y(), -p.z()); } - - /** Syntactic sugar for multiplying coordinates by a scalar s*p */ inline Point3 operator*(double s, const Point3& p) { return p*s;} diff --git a/geometry/Pose2.cpp b/geometry/Pose2.cpp index de20393643..cc0cd556bd 100644 --- a/geometry/Pose2.cpp +++ b/geometry/Pose2.cpp @@ -68,13 +68,15 @@ namespace gtsam { #else - template<> Pose2 expmap(const Vector& v) { - return Pose2(v[0], v[1], v[2]); - } + /* ************************************************************************* */ + Pose2 Pose2::Expmap(const Vector& v) { + return Pose2(v[0], v[1], v[2]); + } - Vector logmap(const Pose2& p) { - return Vector_(3, p.x(), p.y(), p.theta()); - } + /* ************************************************************************* */ + Vector Pose2::Logmap(const Pose2& p) { + return Vector_(3, p.x(), p.y(), p.theta()); + } #endif @@ -93,15 +95,15 @@ namespace gtsam { } /* ************************************************************************* */ - Pose2 inverse(const Pose2& pose) { - const Rot2& R = pose.r(); - const Point2& t = pose.t(); - return Pose2(inverse(R), R.unrotate(Point2(-t.x(), -t.y()))); - } + Pose2 Pose2::inverse() const { + const Rot2& R = r_; + const Point2& t = t_; + return Pose2(R.inverse(), R.unrotate(Point2(-t.x(), -t.y()))); + } - Matrix Dinverse(const Pose2& pose) { - return -AdjointMap(pose); - } + Matrix Dinverse(const Pose2& pose) { + return -AdjointMap(pose); + } /* ************************************************************************* */ // see doc/math.lyx, SE(2) section @@ -187,7 +189,7 @@ namespace gtsam { /* ************************************************************************* */ Rot2 bearing(const Pose2& pose, const Point2& point) { Point2 d = transform_to(pose, point); - return relativeBearing(d); + return Rot2::relativeBearing(d); } Rot2 bearing(const Pose2& pose, const Point2& point, @@ -195,7 +197,7 @@ namespace gtsam { if (!H1 && !H2) return bearing(pose, point); Point2 d = transform_to(pose, point, H1, H2); Matrix D_result_d; - Rot2 result = relativeBearing(d, D_result_d); + Rot2 result = Rot2::relativeBearing(d, D_result_d); if (H1) *H1 = D_result_d * (*H1); if (H2) *H2 = D_result_d * (*H2); return result; diff --git a/geometry/Pose2.h b/geometry/Pose2.h index 4cf9828374..031f605082 100644 --- a/geometry/Pose2.h +++ b/geometry/Pose2.h @@ -22,6 +22,9 @@ namespace gtsam { * A 2D pose (Point2,Rot2) */ class Pose2: Testable, public Lie { + + public: + static const size_t dimension = 3; private: Rot2 r_; Point2 t_; @@ -60,6 +63,34 @@ namespace gtsam { /** assert equality up to a tolerance */ bool equals(const Pose2& pose, double tol = 1e-9) const; + inline Pose2 operator*(const Pose2& p2) const { + return Pose2(r_*p2.r(), t_ + r_*p2.t()); + } + + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /** Lie requirements */ + + /** return DOF, dimensionality of tangent space = 3 */ + inline size_t dim() const { return dimension; } + + /** inverse of a pose */ + Pose2 inverse() const; + + /** compose with another pose */ + inline Pose2 compose(const Pose2& p) const { return *this * p; } + + /** + * Exponential map from se(2) to SE(2) + */ + static Pose2 Expmap(const Vector& v); + + /** + * Inverse of exponential map, from SE(2) to se(2) + */ + static Vector Logmap(const Pose2& p); + /** return transformation matrix */ Matrix matrix() const; @@ -71,8 +102,6 @@ namespace gtsam { inline const Point2& t() const { return t_; } inline const Rot2& r() const { return r_; } - static inline size_t dim() { return 3; }; - private: // Serialization function friend class boost::serialization::access; @@ -83,22 +112,6 @@ namespace gtsam { } }; // Pose2 - /** print using member print function, currently used by LieConfig */ - inline void print(const Pose2& obj, const std::string& str = "") { obj.print(str); } - - /** return DOF, dimensionality of tangent space = 3 */ - inline size_t dim(const Pose2&) { return 3; } - - /** - * Exponential map from se(2) to SE(2) - */ - template<> Pose2 expmap(const Vector& v); - - /** - * Inverse of exponential map, from SE(2) to se(2) - */ - Vector logmap(const Pose2& p); - /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) @@ -128,15 +141,11 @@ namespace gtsam { /** * inverse transformation */ - Pose2 inverse(const Pose2& pose); Matrix Dinverse(const Pose2& pose); /** * compose this transformation onto another (first p1 and then p2) */ - inline Pose2 operator*(const Pose2& p1, const Pose2& p2) { - return Pose2(p1.r()*p2.r(), p1.t() + p1.r()*p2.t()); } - inline Pose2 compose(const Pose2& p1, const Pose2& p2) { return p1*p2; } Pose2 compose(const Pose2& p1, const Pose2& p2, boost::optional H1, boost::optional H2 = boost::none); diff --git a/geometry/Pose3.cpp b/geometry/Pose3.cpp index 9a81e16441..b7ac732a4f 100644 --- a/geometry/Pose3.cpp +++ b/geometry/Pose3.cpp @@ -89,17 +89,19 @@ namespace gtsam { #else + /* ************************************************************************* */ /* incorrect versions for which we know how to compute derivatives */ - template<> Pose3 expmap(const Vector& d) { + Pose3 Pose3::Expmap(const Vector& d) { Vector w = sub(d, 0,3); Vector u = sub(d, 3,6); - return Pose3(expmap (w), expmap (u)); + return Pose3(Rot3::Expmap(w), Point3::Expmap(u)); } + /* ************************************************************************* */ // Log map at identity - return the translation and canonical rotation // coordinates of a pose. - Vector logmap(const Pose3& p) { - const Vector w = logmap(p.rotation()), u = logmap(p.translation()); + Vector Pose3::Logmap(const Pose3& p) { + const Vector w = Rot3::Logmap(p.rotation()), u = Point3::Logmap(p.translation()); return concatVectors(2, &w, &u); } diff --git a/geometry/Pose3.h b/geometry/Pose3.h index 29bc91d151..e2df0e8732 100644 --- a/geometry/Pose3.h +++ b/geometry/Pose3.h @@ -18,6 +18,9 @@ namespace gtsam { /** A 3D pose (R,t) : (Rot3,Point3) */ class Pose3 : Testable, public Lie { + public: + static const size_t dimension = 6; + private: Rot3 R_; Point3 t_; @@ -59,21 +62,37 @@ namespace gtsam { /** assert equality up to a tolerance */ bool equals(const Pose3& pose, double tol = 1e-9) const; - /** Find the inverse pose s.t. inverse(p)*p = Pose3() */ - inline Pose3 inverse() const { - const Rot3 Rt(R_.inverse()); - return Pose3(Rt, - (Rt*t_)); - } - /** Compose two poses */ inline Pose3 operator*(const Pose3& T) const { - return Pose3(R_*T.R_, t_ + R_*T.t_); + return Pose3(R_*T.R_, t_ + R_*T.t_); } Pose3 transform_to(const Pose3& pose) const; - /** get the dimension by the type */ - inline static size_t dim() { return 6; } + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /** Lie requirements */ + + /** Dimensionality of the tangent space */ + inline size_t dim() const { return dimension; } + + /** Find the inverse pose s.t. inverse(p)*p = Pose3() */ + inline Pose3 inverse() const { + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt*(-t_)); + } + + /** composes two poses */ + inline Pose3 compose(const Pose3& t) const { return *this * t; } + + /** Exponential map at identity - create a pose with a translation and + * rotation (in canonical coordinates). */ + static Pose3 Expmap(const Vector& v); + + /** Log map at identity - return the translation and canonical rotation + * coordinates of a pose. */ + static Vector Logmap(const Pose3& p); private: /** Serialization function */ @@ -85,29 +104,6 @@ namespace gtsam { } }; // Pose3 class - /** global print */ - inline void print(const Pose3& p, const std::string& s = "") { p.print(s);} - - /** Dimensionality of the tangent space */ - inline size_t dim(const Pose3&) { return 6; } - - /** Compose two poses */ - inline Pose3 compose(const Pose3& p0, const Pose3& p1) { return p0*p1;} - - /** Find the inverse pose s.t. inverse(p)*p = Pose3() */ - inline Pose3 inverse(const Pose3& p) { - Rot3 Rt = inverse(p.rotation()); - return Pose3(Rt, Rt*(-p.translation())); - } - - /** Exponential map at identity - create a pose with a translation and - * rotation (in canonical coordinates). */ - template<> Pose3 expmap(const Vector& d); - - /** Log map at identity - return the translation and canonical rotation - * coordinates of a pose. */ - Vector logmap(const Pose3& p); - /** Exponential map around another pose */ Pose3 expmap(const Pose3& T, const Vector& d); diff --git a/geometry/Rot2.cpp b/geometry/Rot2.cpp index 1f0170c92e..a325c3acb9 100644 --- a/geometry/Rot2.cpp +++ b/geometry/Rot2.cpp @@ -93,13 +93,13 @@ namespace gtsam { } /* ************************************************************************* */ - Rot2 relativeBearing(const Point2& d) { + Rot2 Rot2::relativeBearing(const Point2& d) { double n = d.norm(); return Rot2::fromCosSin(d.x() / n, d.y() / n); } /* ************************************************************************* */ - Rot2 relativeBearing(const Point2& d, boost::optional H) { + Rot2 Rot2::relativeBearing(const Point2& d, boost::optional H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if (H) *H = Matrix_(1, 2, -y / d2, x / d2); return Rot2::fromCosSin(x / n, y / n); diff --git a/geometry/Rot2.h b/geometry/Rot2.h index 251ea2b66f..9d73181b8f 100644 --- a/geometry/Rot2.h +++ b/geometry/Rot2.h @@ -21,6 +21,10 @@ namespace gtsam { */ class Rot2: Testable, public Lie { + public: + /** get the dimension by the type */ + static const size_t dimension = 1; + private: /** we store cos(theta) and sin(theta) */ @@ -52,6 +56,22 @@ namespace gtsam { static Rot2 fromCosSin(double c, double s); /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ + + /** + * Named constructor + * Calculate relative bearing to a landmark in local coordinate frame + * @param point 2D location of landmark + * @param H optional reference for Jacobian + * @return 2D rotation \in SO(2) + */ + static Rot2 relativeBearing(const Point2& d); + + /** + * Named constructor with derivative + * Calculate relative bearing and optional derivative + */ + static Rot2 relativeBearing(const Point2& d, boost::optional H); + static Rot2 atan2(double y, double x); /** return angle (RADIANS) */ @@ -75,6 +95,28 @@ namespace gtsam { /** equals with an tolerance */ bool equals(const Rot2& R, double tol = 1e-9) const; + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /** Lie requirements */ + + /** Dimensionality of the tangent space */ + inline size_t dim() const { return dimension; } + + /** Compose - make a new rotation by adding angles */ + inline Rot2 compose(const Rot2& R1) const { return *this * R1;} + + /** Expmap around identity - create a rotation from an angle */ + static Rot2 Expmap(const Vector& v) { + if (zero(v)) return (Rot2()); + else return Rot2::fromAngle(v(0)); + } + + /** Logmap around identity - return the angle of the rotation */ + static inline Vector Logmap(const Rot2& r) { + return Vector_(1, r.theta()); + } + /** return 2*2 rotation matrix */ Matrix matrix() const; @@ -95,9 +137,6 @@ namespace gtsam { /** rotate from world to rotated = R'*p */ Point2 unrotate(const Point2& p) const; - /** get the dimension by the type */ - static inline size_t dim() { return 1; }; - private: /** Serialization function */ friend class boost::serialization::access; @@ -116,29 +155,6 @@ namespace gtsam { // Lie group functions - /** Global print calls member function */ - inline void print(const Rot2& r, const std::string& s = "") { r.print(s); } - - /** Dimensionality of the tangent space */ - inline size_t dim(const Rot2&) { return 1; } - - /** Expmap around identity - create a rotation from an angle */ - template<> inline Rot2 expmap(const Vector& v) { - if (zero(v)) return (Rot2()); - else return Rot2::fromAngle(v(0)); - } - - /** Logmap around identity - return the angle of the rotation */ - inline Vector logmap(const Rot2& r) { - return Vector_(1, r.theta()); - } - - /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R1, const Rot2& R2) { return R1*R2;} - - /** The inverse rotation - negative angle */ - inline Rot2 inverse(const Rot2& R) { return R.inverse();} - /** * rotate point from rotated coordinate frame to * world = R*p @@ -154,19 +170,6 @@ namespace gtsam { Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none); - /** - * Calculate relative bearing to a landmark in local coordinate frame - * @param point 2D location of landmark - * @param H optional reference for Jacobian - * @return 2D rotation \in SO(2) - */ - Rot2 relativeBearing(const Point2& d); - - /** - * Calculate relative bearing and optional derivative - */ - Rot2 relativeBearing(const Point2& d, boost::optional H); - } // gtsam #endif /* ROT2_H_ */ diff --git a/geometry/Rot3.cpp b/geometry/Rot3.cpp index ee6d07571c..3210cd1965 100644 --- a/geometry/Rot3.cpp +++ b/geometry/Rot3.cpp @@ -68,6 +68,35 @@ namespace gtsam { ); } + /* ************************************************************************* */ + Rot3 Rot3::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +#ifndef NDEBUG + double l_n = wwTxx + wwTyy + wwTzz; + if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); +#endif + + double c = cos(theta), s = sin(theta), c_1 = 1 - c; + + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; + + return Rot3( c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); + } + + /* ************************************************************************* */ + Rot3 Rot3::rodriguez(const Vector& w) { + double t = norm_2(w); + if (t < 1e-5) return Rot3(); + return rodriguez(w/t, t); + } + /* ************************************************************************* */ bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); @@ -113,7 +142,7 @@ namespace gtsam { /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation - inline Vector logmap(const Rot3& R) { + inline Vector Rot3::Logmap(const Rot3& R) { double tr = R.r1().x()+R.r2().y()+R.r3().z(); if (fabs(tr-3.0) < 1e-10) { // when theta = 0, +-2pi, +-4pi, etc. return zero(3); @@ -136,35 +165,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - Rot3 rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; -#ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); -#endif - - double c = cos(theta), s = sin(theta), c_1 = 1 - c; - - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; - - return Rot3( c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); - } - - /* ************************************************************************* */ - Rot3 rodriguez(const Vector& w) { - double t = norm_2(w); - if (t < 1e-5) return Rot3(); - return rodriguez(w/t, t); - } - /* ************************************************************************* */ Matrix Drotate1(const Rot3& R, const Point3& p) { return R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); diff --git a/geometry/Rot3.h b/geometry/Rot3.h index 700d221c23..85abbe2aee 100644 --- a/geometry/Rot3.h +++ b/geometry/Rot3.h @@ -19,6 +19,9 @@ namespace gtsam { /* 3D Rotation */ class Rot3: Testable, public Lie { + public: + static const size_t dimension = 3; + private: /** we store columns ! */ Point3 r1_, r2_, r3_; @@ -76,6 +79,31 @@ namespace gtsam { static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis, unit length + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Vector& w, double theta); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param v a vector of incremental roll,pitch,yaw + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Vector& v); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param wx + * @param wy + * @param wz + * @return incremental rotation matrix + */ + static inline Rot3 rodriguez(double wx, double wy, double wz) + { return rodriguez(Vector_(3,wx,wy,wz));} + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} @@ -106,8 +134,27 @@ namespace gtsam { */ Vector ypr() const; - /** get the dimension by the type */ - static inline size_t dim() { return 3; }; + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /** Lie requirements */ + + /** return DOF, dimensionality of tangent space */ + inline size_t dim() const { return dimension; } + + /** Compose two rotations */ + inline Rot3 compose(const Rot3& R1) const { return *this * R1;} + + /** Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3 Expmap(const Vector& v) { + if(zero(v)) return Rot3(); + else return rodriguez(v); + } + + // Log map at identity - return the canonical coordinates of this rotation + static Vector Logmap(const Rot3& R); /* Find the inverse rotation R^T s.t. inverse(R)*R = I */ inline Rot3 inverse() const { @@ -142,55 +189,7 @@ namespace gtsam { } }; - /** Global print calls member function */ - inline void print(const Rot3& r, std::string& s) { r.print(s); } - inline void print(const Rot3& r) { r.print(); } - - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param w is the rotation axis, unit length - * @param theta rotation angle - * @return incremental rotation matrix - */ - Rot3 rodriguez(const Vector& w, double theta); - - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param v a vector of incremental roll,pitch,yaw - * @return incremental rotation matrix - */ - Rot3 rodriguez(const Vector& v); - - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param wx - * @param wy - * @param wz - * @return incremental rotation matrix - */ - inline Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector_(3,wx,wy,wz));} - - /** return DOF, dimensionality of tangent space */ - inline size_t dim(const Rot3&) { return 3; } - - // Exponential map at identity - create a rotation from canonical coordinates - // using Rodriguez' formula - template<> inline Rot3 expmap(const Vector& v) { - if(zero(v)) return Rot3(); - else return rodriguez(v); - } - - // Log map at identity - return the canonical coordinates of this rotation - Vector logmap(const Rot3& R); - - // Compose two rotations - inline Rot3 compose(const Rot3& R1, const Rot3& R2) { return R1*R2;} - - // Find the inverse rotation R^T s.t. inverse(R)*R = Rot3() - inline Rot3 inverse(const Rot3& R) { return R.inverse();} - - // and its derivative + // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() inline Matrix Dinverse(Rot3 R) { return -R.matrix();} /** diff --git a/geometry/StereoPoint2.h b/geometry/StereoPoint2.h index 621ccad9d0..78e298add3 100644 --- a/geometry/StereoPoint2.h +++ b/geometry/StereoPoint2.h @@ -20,6 +20,8 @@ namespace gtsam { * A 2D stereo point, v will be same for rectified images */ class StereoPoint2: Testable, Lie { + public: + static const size_t dimension = 3; private: double uL_, uR_, v_; @@ -47,6 +49,12 @@ namespace gtsam { - q.v_) < tol); } + /** dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /** Lie requirements */ + inline size_t dim() const { return dimension; } + /** convert to vector */ Vector vector() const { return Vector_(3, uL_, uR_, v_); @@ -68,33 +76,27 @@ namespace gtsam { inline Point2 point2(){ return Point2(uL_, v_); } - }; - - /** Dimensionality of the tangent space */ - inline size_t dim(const StereoPoint2& obj) { - return 3; - } - - /** Exponential map around identity - just create a Point2 from a vector */ - template<> inline StereoPoint2 expmap(const Vector& d) { - return StereoPoint2(d(0), d(1), d(2)); - } - /** Log map around identity - just return the Point2 as a vector */ - inline Vector logmap(const StereoPoint2& p) { - return p.vector(); - } + /** "Compose", just adds the coordinates of two points. */ + inline StereoPoint2 compose(const StereoPoint2& p1) const { + return *this + p1; + } - /** "Compose", just adds the coordinates of two points. */ - inline StereoPoint2 compose(const StereoPoint2& p1, const StereoPoint2& p0) { - return p0 + p1; - } + /** inverse */ + inline StereoPoint2 inverse() const { + return StereoPoint2()- (*this); + } - /** inverse */ - inline StereoPoint2 inverse(const StereoPoint2& p) { - return StereoPoint2()-p; - } + /** Exponential map around identity - just create a Point2 from a vector */ + static inline StereoPoint2 Expmap(const Vector& d) { + return StereoPoint2(d(0), d(1), d(2)); + } + /** Log map around identity - just return the Point2 as a vector */ + static inline Vector Logmap(const StereoPoint2& p) { + return p.vector(); + } + }; } diff --git a/geometry/tests/testPose3.cpp b/geometry/tests/testPose3.cpp index 28199167db..f2d5b61ba0 100644 --- a/geometry/tests/testPose3.cpp +++ b/geometry/tests/testPose3.cpp @@ -12,10 +12,10 @@ using namespace std; using namespace gtsam; static Point3 P(0.2,0.7,-2); -static Rot3 R = rodriguez(0.3,0,0); +static Rot3 R = Rot3::rodriguez(0.3,0,0); static Pose3 T(R,Point3(3.5,-8.2,4.2)); -static Pose3 T2(rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); -static Pose3 T3(rodriguez(-90, 0, 0), Point3(1, 2, 3)); +static Pose3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); +static Pose3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3)); /* ************************************************************************* */ TEST( Pose3, equals) @@ -43,12 +43,13 @@ TEST( Pose3, expmap_a) CHECK(assert_equal(Pose3(R, P),expmap(id,v),1e-5)); } +/* ************************************************************************* */ TEST(Pose3, expmap_b) { Pose3 p1(Rot3(), Point3(100, 0, 0)); Pose3 p2 = expmap(p1, Vector_(6, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0)); - Pose3 expected(rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); + Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0)); CHECK(assert_equal(expected, p2)); } @@ -178,6 +179,7 @@ TEST( Pose3, compose2 ) Matrix numericalH2 = numericalDerivative22(compose, T1, T2, 1e-5); CHECK(assert_equal(numericalH2,actualDcompose2)); } + /* ************************************************************************* */ TEST( Pose3, inverse) { @@ -193,7 +195,7 @@ TEST( Pose3, inverse) /* ************************************************************************* */ TEST( Pose3, inverseDerivatives2) { - Rot3 R = rodriguez(0.3,0.4,-0.5); + Rot3 R = Rot3::rodriguez(0.3,0.4,-0.5); Point3 t(3.5,-8.2,4.2); Pose3 T(R,t); @@ -298,25 +300,25 @@ TEST( Pose3, transform_to_translate) { Point3 actual = transform_to(Pose3(Rot3(), Point3(1, 2, 3)), Point3(10.,20.,30.)); Point3 expected(9.,18.,27.); - CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST( Pose3, transform_to_rotate) { - Pose3 transform(rodriguez(0,0,-1.570796), Point3()); + Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3()); Point3 actual = transform_to(transform, Point3(2,1,10)); Point3 expected(-1,2,10); - CHECK(assert_equal(expected, actual, 0.001)); + CHECK(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ TEST( Pose3, transform_to) { - Pose3 transform(rodriguez(0,0,-1.570796), Point3(2,4, 0)); + Pose3 transform(Rot3::rodriguez(0,0,-1.570796), Point3(2,4, 0)); Point3 actual = transform_to(transform, Point3(3,2,10)); Point3 expected(2,1,10); - CHECK(assert_equal(expected, actual, 0.001)); + CHECK(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ @@ -355,10 +357,10 @@ TEST( Pose3, transformPose_to_itself) TEST( Pose3, transformPose_to_translation) { // transform translation only - Rot3 r = rodriguez(-1.570796,0,0); - Pose3 pose2(r, Point3(21.,32.,13.)); + Rot3 r = Rot3::rodriguez(-1.570796,0,0); + Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); - Pose3 expected(r, Point3(20.,30.,10.)); + Pose3 expected(r, Point3(20.,30.,10.)); CHECK(assert_equal(expected, actual, 1e-8)); } @@ -366,25 +368,25 @@ TEST( Pose3, transformPose_to_translation) TEST( Pose3, transformPose_to_simple_rotate) { // transform translation only - Rot3 r = rodriguez(0,0,-1.570796); - Pose3 pose2(r, Point3(21.,32.,13.)); + Rot3 r = Rot3::rodriguez(0,0,-1.570796); + Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 transform(r, Point3(1,2,3)); Pose3 actual = pose2.transform_to(transform); - Pose3 expected(Rot3(), Point3(-30.,20.,10.)); - CHECK(assert_equal(expected, actual, 0.001)); + Pose3 expected(Rot3(), Point3(-30.,20.,10.)); + CHECK(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ TEST( Pose3, transformPose_to) { // transform to - Rot3 r = rodriguez(0,0,-1.570796); //-90 degree yaw - Rot3 r2 = rodriguez(0,0,0.698131701); //40 degree yaw - Pose3 pose2(r2, Point3(21.,32.,13.)); + Rot3 r = Rot3::rodriguez(0,0,-1.570796); //-90 degree yaw + Rot3 r2 = Rot3::rodriguez(0,0,0.698131701); //40 degree yaw + Pose3 pose2(r2, Point3(21.,32.,13.)); Pose3 transform(r, Point3(1,2,3)); Pose3 actual = pose2.transform_to(transform); - Pose3 expected(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); - CHECK(assert_equal(expected, actual, 0.001)); + Pose3 expected(Rot3::rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); + CHECK(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ diff --git a/geometry/tests/testRot2.cpp b/geometry/tests/testRot2.cpp index f2e8b9bace..721371b799 100644 --- a/geometry/tests/testRot2.cpp +++ b/geometry/tests/testRot2.cpp @@ -95,19 +95,19 @@ TEST( Rot2, relativeBearing ) Matrix expectedH, actualH; // establish relativeBearing is indeed zero - Rot2 actual1 = relativeBearing(l1, actualH); + Rot2 actual1 = Rot2::relativeBearing(l1, actualH); CHECK(assert_equal(Rot2(),actual1)); // Check numerical derivative - expectedH = numericalDerivative11(relativeBearing, l1, 1e-5); + expectedH = numericalDerivative11(Rot2::relativeBearing, l1, 1e-5); CHECK(assert_equal(expectedH,actualH)); // establish relativeBearing is indeed 45 degrees - Rot2 actual2 = relativeBearing(l2, actualH); + Rot2 actual2 = Rot2::relativeBearing(l2, actualH); CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2)); // Check numerical derivative - expectedH = numericalDerivative11(relativeBearing, l2, 1e-5); + expectedH = numericalDerivative11(Rot2::relativeBearing, l2, 1e-5); CHECK(assert_equal(expectedH,actualH)); } diff --git a/geometry/tests/testRot3.cpp b/geometry/tests/testRot3.cpp index 6791e36e02..3cb834f034 100644 --- a/geometry/tests/testRot3.cpp +++ b/geometry/tests/testRot3.cpp @@ -12,7 +12,7 @@ using namespace gtsam; -Rot3 R = rodriguez(0.1, 0.4, 0.2); +Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Point3 P(0.2, 0.7, -2.0); double error = 1e-9, epsilon = 0.001; @@ -80,7 +80,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { /* ************************************************************************* */ TEST( Rot3, rodriguez) { - Rot3 R1 = rodriguez(epsilon, 0, 0); + Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); Vector w = Vector_(3, epsilon, 0., 0.); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -91,7 +91,7 @@ TEST( Rot3, rodriguez2) { Vector axis = Vector_(3,0.,1.,0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3 actual = rodriguez(axis, angle); + Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); @@ -102,7 +102,7 @@ TEST( Rot3, rodriguez2) TEST( Rot3, rodriguez3) { Vector w = Vector_(3, 0.1, 0.2, 0.3); - Rot3 R1 = rodriguez(w / norm_2(w), norm_2(w)); + Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } @@ -112,7 +112,7 @@ TEST( Rot3, rodriguez4) { Vector axis = Vector_(3,0.,0.,1.); // rotation around Z double angle = M_PI_2; - Rot3 actual = rodriguez(axis, angle); + Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); Rot3 expected(c,-s, 0, s, c, 0, @@ -133,43 +133,43 @@ TEST( Rot3, expmap) TEST(Rot3, log) { Vector w1 = Vector_(3, 0.1, 0.0, 0.0); - Rot3 R1 = rodriguez(w1); + Rot3 R1 = Rot3::rodriguez(w1); CHECK(assert_equal(w1, logmap(R1))); Vector w2 = Vector_(3, 0.0, 0.1, 0.0); - Rot3 R2 = rodriguez(w2); + Rot3 R2 = Rot3::rodriguez(w2); CHECK(assert_equal(w2, logmap(R2))); Vector w3 = Vector_(3, 0.0, 0.0, 0.1); - Rot3 R3 = rodriguez(w3); + Rot3 R3 = Rot3::rodriguez(w3); CHECK(assert_equal(w3, logmap(R3))); Vector w = Vector_(3, 0.1, 0.4, 0.2); - Rot3 R = rodriguez(w); + Rot3 R = Rot3::rodriguez(w); CHECK(assert_equal(w, logmap(R))); Vector w5 = Vector_(3, 0.0, 0.0, 0.0); - Rot3 R5 = rodriguez(w5); + Rot3 R5 = Rot3::rodriguez(w5); CHECK(assert_equal(w5, logmap(R5))); Vector w6 = Vector_(3, boost::math::constants::pi(), 0.0, 0.0); - Rot3 R6 = rodriguez(w6); + Rot3 R6 = Rot3::rodriguez(w6); CHECK(assert_equal(w6, logmap(R6))); Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); - Rot3 R7 = rodriguez(w7); + Rot3 R7 = Rot3::rodriguez(w7); CHECK(assert_equal(w7, logmap(R7))); Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); - Rot3 R8 = rodriguez(w8); + Rot3 R8 = Rot3::rodriguez(w8); CHECK(assert_equal(w8, logmap(R8))); } /* ************************************************************************* */ TEST(Rot3, manifold) { - Rot3 gR1 = rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = rodriguez(0.3, 0.1, 0.7); + Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 origin; // log behaves correctly @@ -265,8 +265,8 @@ TEST( Rot3, unrotate) /* ************************************************************************* */ TEST( Rot3, compose ) { - Rot3 R1 = rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); Rot3 expected = R1 * R2; Rot3 actual = compose(R1, R2); @@ -287,7 +287,7 @@ TEST( Rot3, compose ) TEST( Rot3, inverse ) { - Rot3 R = rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 I; CHECK(assert_equal(I,R*inverse(R))); @@ -301,13 +301,13 @@ TEST( Rot3, inverse ) /* ************************************************************************* */ TEST( Rot3, between ) { - Rot3 R = rodriguez(0.1, 0.4, 0.2); + Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 origin; CHECK(assert_equal(R, between(origin,R))); CHECK(assert_equal(inverse(R), between(R,origin))); - Rot3 R1 = rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); Rot3 expected = inverse(R1) * R2; Rot3 actual = between(R1, R2); diff --git a/geometry/timeRot3.cpp b/geometry/timeRot3.cpp index d67e328d6d..0b50f4cc21 100644 --- a/geometry/timeRot3.cpp +++ b/geometry/timeRot3.cpp @@ -20,7 +20,7 @@ int main() // Rodriguez formula given axis angle long timeLog = clock(); for(int i = 0; i < n; i++) - rodriguez(v,0.001); + Rot3::rodriguez(v,0.001); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; cout << seconds << " seconds" << endl; @@ -29,7 +29,7 @@ int main() // Rodriguez formula given canonical coordinates timeLog = clock(); for(int i = 0; i < n; i++) - rodriguez(v); + Rot3::rodriguez(v); timeLog2 = clock(); seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; cout << seconds << " seconds" << endl; diff --git a/nonlinear/LieConfig-inl.h b/nonlinear/LieConfig-inl.h index a7cfa19d78..1c38fe9bdb 100644 --- a/nonlinear/LieConfig-inl.h +++ b/nonlinear/LieConfig-inl.h @@ -32,8 +32,9 @@ namespace gtsam { template void LieConfig::print(const string &s) const { cout << "LieConfig " << s << ", size " << values_.size() << "\n"; - BOOST_FOREACH(const typename Values::value_type& v, values_) + BOOST_FOREACH(const typename Values::value_type& v, values_) { gtsam::print(v.second, (string)(v.first)); + } } template diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index 0748c81290..f84df016ed 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -399,12 +399,12 @@ class NonlinearEquality1 : public NonlinearEqualityConstraint1 { typedef boost::shared_ptr > shared_ptr; NonlinearEquality1(const X& value, const Key& key1, double mu = 1000.0) - : Base(key1, X::dim(), mu), value_(value) {} + : Base(key1, X::dimension, mu), value_(value) {} virtual ~NonlinearEquality1() {} /** g(x) with optional derivative */ Vector evaluateError(const X& x1, boost::optional H1 = boost::none) const { - const size_t p = X::dim(); + const size_t p = X::dimension; if (H1) *H1 = eye(p); return logmap(value_, x1); } @@ -425,14 +425,14 @@ class NonlinearEquality2 : public NonlinearEqualityConstraint2 > shared_ptr; NonlinearEquality2(const Key& key1, const Key& key2, double mu = 1000.0) - : Base(key1, key2, X::dim(), mu) {} + : Base(key1, key2, X::dimension, mu) {} virtual ~NonlinearEquality2() {} /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - const size_t p = X::dim(); + const size_t p = X::dimension; if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); return logmap(x1, x2); diff --git a/slam/BetweenConstraint.h b/slam/BetweenConstraint.h index e614eda700..9fd9dcee15 100644 --- a/slam/BetweenConstraint.h +++ b/slam/BetweenConstraint.h @@ -26,7 +26,7 @@ namespace gtsam { typedef boost::shared_ptr > shared_ptr; BetweenConstraint(const X& measured, const Key& key1, const Key& key2, double mu = 1000.0) - : Base(key1, key2, X::dim(), mu), measured_(measured) {} + : Base(key1, key2, X::dimension, mu), measured_(measured) {} /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, diff --git a/slam/tests/testPose3Factor.cpp b/slam/tests/testPose3Factor.cpp index 0acf5d9d6f..4ed208775e 100644 --- a/slam/tests/testPose3Factor.cpp +++ b/slam/tests/testPose3Factor.cpp @@ -19,8 +19,8 @@ TEST( Pose3Factor, error ) { // Create example Pose3 t1; // origin - Pose3 t2(rodriguez(0.1,0.2,0.3),Point3(0,1,0)); - Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; + Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0)); + Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; // Create factor SharedGaussian I6(noiseModel::Unit::Create(6)); diff --git a/slam/tests/testSimulated3D.cpp b/slam/tests/testSimulated3D.cpp index 905b71b0b9..f18e5340d7 100644 --- a/slam/tests/testSimulated3D.cpp +++ b/slam/tests/testSimulated3D.cpp @@ -17,7 +17,7 @@ using namespace simulated3D; /* ************************************************************************* */ TEST( simulated3D, Dprior ) { - Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); + Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0)); Vector v = logmap(x1); Matrix numerical = numericalDerivative11(prior,v); Matrix computed = Dprior(v); @@ -27,9 +27,9 @@ TEST( simulated3D, Dprior ) /* ************************************************************************* */ TEST( simulated3D, DOdo1 ) { - Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); + Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0)); Vector v1 = logmap(x1); - Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); + Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0)); Vector v2 = logmap(x2); Matrix numerical = numericalDerivative21(odo,v1,v2); Matrix computed = Dodo1(v1,v2); @@ -39,9 +39,9 @@ TEST( simulated3D, DOdo1 ) /* ************************************************************************* */ TEST( simulated3D, DOdo2 ) { - Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0)); + Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0)); Vector v1 = logmap(x1); - Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0)); + Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0)); Vector v2 = logmap(x2); Matrix numerical = numericalDerivative22(odo,v1,v2); Matrix computed = Dodo2(v1,v2);