From 971a53cfb56591914a4493e6753390f46cae2fa4 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 7 Dec 2014 12:39:53 -0500 Subject: [PATCH] Fixed size vectors for all Logmap, localcoordinates and vector methods --- gtsam/base/Vector.h | 2 ++ gtsam/geometry/Cal3Bundler.cpp | 2 +- gtsam/geometry/Cal3DS2_Base.cpp | 6 ++++-- gtsam/geometry/Cal3DS2_Base.h | 4 ++-- gtsam/geometry/Cal3Unified.cpp | 8 +++++--- gtsam/geometry/Cal3Unified.h | 4 ++-- gtsam/geometry/EssentialMatrix.cpp | 8 +++++--- gtsam/geometry/EssentialMatrix.h | 10 +++++----- gtsam/geometry/Point2.h | 2 +- gtsam/geometry/Pose2.cpp | 10 +++++----- gtsam/geometry/Pose2.h | 13 +++++++------ gtsam/geometry/Rot2.h | 8 +++++--- gtsam/geometry/Rot3.cpp | 2 +- gtsam/geometry/Rot3.h | 4 ++-- gtsam/geometry/StereoCamera.cpp | 8 ++++---- gtsam/geometry/StereoCamera.h | 10 ++-------- gtsam/geometry/Unit3.cpp | 2 +- 17 files changed, 54 insertions(+), 49 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 1be8408d2..74cd248a1 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -34,6 +34,7 @@ namespace gtsam { typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors +typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Matrix Vector4; @@ -42,6 +43,7 @@ typedef Eigen::Matrix Vector6; typedef Eigen::Matrix Vector7; typedef Eigen::Matrix Vector8; typedef Eigen::Matrix Vector9; +typedef Eigen::Matrix Vector10; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index ccd061d7c..368ae6c98 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -49,7 +49,7 @@ Vector4 Cal3Bundler::k() const { /* ************************************************************************* */ Vector3 Cal3Bundler::vector() const { - return (Vector(3) << f_, k1_, k2_).finished(); + return Vector3(f_, k1_, k2_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index cfbce2b28..12060c12d 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -35,8 +35,10 @@ Matrix3 Cal3DS2_Base::K() const { } /* ************************************************************************* */ -Vector Cal3DS2_Base::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished(); +Vector9 Cal3DS2_Base::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_; + return v; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index f9292d4f6..37c156743 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -46,8 +46,8 @@ protected: public: Matrix3 K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } - Vector vector() const ; + Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } + Vector9 vector() const ; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 5bdf89856..8b7c1abf4 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v): Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ -Vector Cal3Unified::vector() const { - return (Vector(10) << Base::vector(), xi_).finished(); +Vector10 Cal3Unified::vector() const { + Vector10 v; + v << Base::vector(), xi_; + return v; } /* ************************************************************************* */ @@ -133,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const { } /* ************************************************************************* */ -Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { +Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 133e330ba..e2aa3fc8b 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -51,7 +51,7 @@ private: public: - Vector vector() const ; + Vector10 vector() const ; /// @name Standard Constructors /// @{ @@ -116,7 +116,7 @@ public: Cal3Unified retract(const Vector& d) const ; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Unified& T2) const ; + Vector10 localCoordinates(const Cal3Unified& T2) const ; /// Return dimensions of calibration manifold object virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index c2b690496..062178fea 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -51,9 +51,11 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { } /* ************************************************************************* */ -Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates( - other.aTb_)).finished(); +Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { + Vector5 v; + v << aRb_.localCoordinates(other.aRb_), + aTb_.localCoordinates(other.aTb_); + return v; } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index b25286412..546e432b9 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -31,8 +31,8 @@ private: public: /// Static function to convert Point2 to homogeneous coordinates - static Vector Homogeneous(const Point2& p) { - return (Vector(3) << p.x(), p.y(), 1).finished(); + static Vector3 Homogeneous(const Point2& p) { + return Vector3(p.x(), p.y(), 1); } /// @name Constructors and named constructors @@ -84,15 +84,15 @@ public: } /// Return the dimensionality of the tangent space - virtual size_t dim() const { + size_t dim() const { return 5; } /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const; + EssentialMatrix retract(const Vector& xi) const; /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const; + Vector5 localCoordinates(const EssentialMatrix& other) const; /// @} diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 8c8e03db4..5dabc4bd6 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -171,7 +171,7 @@ public: 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()).finished(); } + static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); } /// @} /// @name Vector Space diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index c9aab4185..edc16af03 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -75,18 +75,18 @@ Pose2 Pose2::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector Pose2::Logmap(const Pose2& p) { +Vector3 Pose2::Logmap(const Pose2& p) { const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) - return (Vector(3) << t.x(), t.y(), w).finished(); + return Vector3(t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; - return (Vector(3) << v.x(), v.y(), w).finished(); + return Vector3(v.x(), v.y(), w); } } @@ -101,12 +101,12 @@ Pose2 Pose2::retract(const Vector& v) const { } /* ************************************************************************* */ -Vector Pose2::localCoordinates(const Pose2& p2) const { +Vector3 Pose2::localCoordinates(const Pose2& p2) const { #ifdef SLOW_BUT_CORRECT_EXPMAP return Logmap(between(p2)); #else Pose2 r = between(p2); - return (Vector(3) << r.x(), r.y(), r.theta()).finished(); + return Vector3(r.x(), r.y(), r.theta()); #endif } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 49eb444b2..a48be51cf 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -139,7 +139,7 @@ public: Pose2 retract(const Vector& v) const; /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose - Vector localCoordinates(const Pose2& p2) const; + Vector3 localCoordinates(const Pose2& p2) const; /// @} /// @name Lie Group @@ -149,7 +149,7 @@ public: static Pose2 Expmap(const Vector& xi); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - static Vector Logmap(const Pose2& p); + static Vector3 Logmap(const Pose2& p); /** * Calculate Adjoint map @@ -169,10 +169,11 @@ public: * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ static inline Matrix3 wedge(double vx, double vy, double w) { - return (Matrix(3,3) << - 0.,-w, vx, - w, 0., vy, - 0., 0., 0.).finished(); + Matrix3 m; + m << 0.,-w, vx, + w, 0., vy, + 0., 0., 0.; + return m; } /// @} diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 744e246b3..d5ea6afdc 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -154,7 +154,7 @@ namespace gtsam { inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } /// Returns inverse retraction - inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } /// @} /// @name Lie Group @@ -169,8 +169,10 @@ namespace gtsam { } ///Log map at identity - return the canonical coordinates of this rotation - static inline Vector Logmap(const Rot2& r) { - return (Vector(1) << r.theta()).finished(); + static inline Vector1 Logmap(const Rot2& r) { + Vector1 v; + v << r.theta(); + return v; } /// @} diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 232661360..0b88a70c7 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -52,7 +52,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w) { +Rot3 Rot3::rodriguez(const Vector3& w) { double t = w.norm(); if (t < 1e-10) return Rot3(); return rodriguez(w/t, t); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index edd39619f..6fe3f92b4 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -183,7 +183,7 @@ namespace gtsam { * @param v a vector of incremental roll,pitch,yaw * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector& v); + static Rot3 rodriguez(const Vector3& v); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -193,7 +193,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez((Vector(3) << wx, wy, wz).finished());} + { return rodriguez(Vector3(wx, wy, wz));} /// @} /// @name Testable diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index cd0c8ee9a..9170f8282 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -91,11 +91,11 @@ namespace gtsam { double d = 1.0 / P.z(), d2 = d*d; const Cal3_S2Stereo& K = *K_; double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - return (Matrix(3, 3) << - f_x*d, 0.0, -d2*f_x* P.x(), + Matrix3 m; + m << f_x*d, 0.0, -d2*f_x* P.x(), f_x*d, 0.0, -d2*f_x*(P.x() - b), - 0.0, f_y*d, -d2*f_y* P.y() - ).finished(); + 0.0, f_y*d, -d2*f_y* P.y(); + return m; } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index b69da71bd..7650aa086 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -90,14 +90,8 @@ public: } /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const StereoCamera& t2) const { - return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_)); - } - - Pose3 between(const StereoCamera &camera, - OptionalJacobian<6,6> H1=boost::none, - OptionalJacobian<6,6> H2=boost::none) const { - return leftCamPose_.between(camera.pose(), H1, H2); + inline Vector6 localCoordinates(const StereoCamera& t2) const { + return leftCamPose_.localCoordinates(t2.leftCamPose_); } /// @} diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 7471dca9d..5b4a8b33e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -164,7 +164,7 @@ Vector2 Unit3::localCoordinates(const Unit3& y) const { if (std::abs(dot - 1.0) < 1e-16) return Vector2(0, 0); else if (std::abs(dot + 1.0) < 1e-16) - return (Vector(2) << M_PI, 0).finished(); + return Vector2(M_PI, 0); else { // no special case double theta = acos(dot);