From d95e1738d463968f31087333a2f7f01e89cbc600 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 9 May 2012 17:19:01 +0000 Subject: [PATCH] fixed size improvements for significant speedup in LBA --- gtsam/base/Matrix.h | 2 ++ gtsam/geometry/Pose3.cpp | 66 ++++++++++++++++++++++++---------------- gtsam/geometry/Pose3.h | 8 ++--- 3 files changed, 45 insertions(+), 31 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index a73c85c1b..693f1e3da 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -38,6 +38,8 @@ typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; typedef Eigen::Matrix3d Matrix3; +typedef Eigen::Matrix4d Matrix4; +typedef Eigen::Matrix Matrix6; // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9f343cbe4..59e5ceef5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -31,7 +31,8 @@ namespace gtsam { /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); - static const Matrix I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3, I6 = eye(6); + static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3; + static const Matrix6 I6 = eye(6); /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : @@ -43,13 +44,13 @@ namespace gtsam { // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) // Experimental - unit tests of derivatives based on it do not check out yet - Matrix Pose3::adjointMap() const { - const Matrix R = R_.matrix(); - const Vector t = t_.vector(); - Matrix A = skewSymmetric(t)*R; - Matrix DR = collect(2, &R, &Z3); - Matrix Dt = collect(2, &A, &R); - return gtsam::stack(2, &DR, &Dt); + Matrix6 Pose3::adjointMap() const { + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + Matrix3 A = skewSymmetric(t)*R; + Matrix6 adj; + adj << R, Z3, A, R; + return adj; } /* ************************************************************************* */ @@ -86,25 +87,30 @@ namespace gtsam { } /* ************************************************************************* */ - Vector Pose3::Logmap(const Pose3& p) { - Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); + Vector6 Pose3::Logmap(const Pose3& p) { + Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); - if (t < 1e-10) - return concatVectors(2, &w, &T); + if (t < 1e-10) { + Vector6 log; + log << w, T; + return log; + } else { - Matrix W = skewSymmetric(w/t); + Matrix3 W = skewSymmetric(w/t); // Formula from Agrawal06iros, equation (14) // simplified with Mathematica, and multiplying in T to avoid matrix math double Tan = tan(0.5*t); - Vector WT = W*T; - Vector u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT); - return concatVectors(2, &w, &u); + Vector3 WT = W*T; + Vector3 u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT); + Vector6 log; + log << w, u; + return log; } } /* ************************************************************************* */ Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector omega(sub(xi, 0, 3)); + Vector3 omega(sub(xi, 0, 3)); Point3 v(sub(xi, 3, 6)); Rot3 R = R_.retract(omega); // R is done exactly Point3 t = t_ + R_ * v; // First order t approximation @@ -130,7 +136,7 @@ namespace gtsam { /* ************************************************************************* */ // different versions of localCoordinates - Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { + Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { if(mode == Pose3::EXPMAP) { // Lie group logarithm map, exact inverse of exponential map return Logmap(between(T)); @@ -146,8 +152,9 @@ namespace gtsam { Point3 d = R_.unrotate(T.translation() - t_); // TODO: correct second order t inverse - - return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z()); + Vector6 local; + local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z(); + return local; } else { assert(false); exit(1); @@ -155,11 +162,14 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix Pose3::matrix() const { - const Matrix R = R_.matrix(), T = Matrix_(3,1, t_.vector()); - const Matrix A34 = collect(2, &R, &T); - const Matrix A14 = Matrix_(1,4, 0.0, 0.0, 0.0, 1.0); - return gtsam::stack(2, &A34, &A14); + Matrix4 Pose3::matrix() const { + const Matrix3 R = R_.matrix(); + const Vector3 T = t_.vector(); + Eigen::Matrix A14; + A14 << 0.0, 0.0, 0.0, 1.0; + Matrix4 mat; + mat << R, T, A14; + return mat; } /* ************************************************************************* */ @@ -175,7 +185,8 @@ namespace gtsam { if (H1) { const Matrix R = R_.matrix(); Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); - *H1 = collect(2,&DR,&R); + H1->resize(3,6); + (*H1) << DR, R; } if (H2) *H2 = R_.matrix(); return R_ * p + t_; @@ -188,7 +199,8 @@ namespace gtsam { if (H1) { const Point3& q = result; Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); - *H1 = collect(2, &DR, &_I3); + H1->resize(3,6); + (*H1) << DR, _I3; } if (H2) *H2 = R_.transpose(); return result; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2d7e42363..29c4a90ca 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -132,7 +132,7 @@ namespace gtsam { Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; /// Local 6D coordinates of Pose3 manifold neighborhood around current pose - Vector localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; + Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; /// @} /// @name Lie Group @@ -142,13 +142,13 @@ namespace gtsam { static Pose3 Expmap(const Vector& xi); /// Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Pose3& p); + static Vector6 Logmap(const Pose3& p); /** * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) */ - Matrix adjointMap() const; /// FIXME Not tested - marked as incorrect + Matrix6 adjointMap() const; /// FIXME Not tested - marked as incorrect Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect /** @@ -201,7 +201,7 @@ namespace gtsam { double z() const { return t_.z(); } /** convert to 4*4 matrix */ - Matrix matrix() const; + Matrix4 matrix() const; /** receives a pose in world coordinates and transforms it to local coordinates */ Pose3 transform_to(const Pose3& pose) const;