From 1623b8ce12adfb64cfdf68e191180867b5296f58 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 11 Apr 2012 06:46:19 +0000 Subject: [PATCH] converted Rot3M to fixed-size Matrix, and changed some methods elsewhere to return fixed-size Vector3 to avoid heap allocations for speedup. --- gtsam/base/Matrix.h | 2 + gtsam/base/Vector.h | 4 + gtsam/geometry/Point3.h | 9 +-- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/Rot3.h | 40 ++++++---- gtsam/geometry/Rot3M.cpp | 163 ++++++++++++++++----------------------- gtsam/geometry/Rot3Q.cpp | 32 ++++---- 7 files changed, 122 insertions(+), 130 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index fee61f62b..4fe3a56c4 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,6 +37,8 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; +typedef Eigen::Matrix3d Matrix3; + // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index abfa44364..2dd038e42 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -33,6 +33,10 @@ namespace gtsam { // Typedef arbitary length vector typedef Eigen::VectorXd Vector; +// Commonly used fixed size vectors +typedef Eigen::Vector3d Vector3; +typedef Eigen::Matrix Vector6; + typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index ad66a845c..9eeb748db 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -126,7 +126,7 @@ namespace gtsam { inline Point3 retract(const Vector& v) const { return Point3(*this + v); } /// Returns inverse retraction - inline Vector localCoordinates(const Point3& q) const { return (q -*this).vector(); } + inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } /// @} /// @name Lie Group @@ -136,7 +136,7 @@ namespace gtsam { 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()); } + static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } /// @} /// @name Vector Space @@ -170,9 +170,8 @@ namespace gtsam { bool operator ==(const Point3& q) const; /** return vectorized form (column-wise)*/ - Vector vector() const { - Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_; - return v; + Vector3 vector() const { + return Vector3(x_,y_,z_); } /// get x diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e12ad28e1..9f343cbe4 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -136,7 +136,7 @@ namespace gtsam { return Logmap(between(T)); } else if(mode == Pose3::FIRST_ORDER) { // R is always done exactly in all three retract versions below - Vector omega = R_.localCoordinates(T.rotation()); + Vector3 omega = R_.localCoordinates(T.rotation()); // Incorrect version // Independently computes the logmap of the translation and rotation diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 599e6a8e9..a948fd5d9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -33,6 +33,7 @@ #endif #include +#include #include #include @@ -59,8 +60,7 @@ namespace gtsam { /** Internal Eigen Quaternion */ Quaternion quaternion_; #else - /** We store columns ! */ - Point3 r1_, r2_, r3_; + Matrix3 rot_; #endif public: @@ -87,6 +87,9 @@ namespace gtsam { /** constructor from a rotation matrix */ Rot3(const Matrix& R); +// /** constructor from a fixed size rotation matrix */ +// Rot3(const Matrix3& R); + /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion * @param q The quaternion @@ -183,7 +186,7 @@ namespace gtsam { /// @{ /** print */ - void print(const std::string& s="R") const { gtsam::print(matrix(), s);} + void print(const std::string& s="R") const { gtsam::print((Matrix)matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3& p, double tol = 1e-9) const; @@ -250,7 +253,7 @@ namespace gtsam { Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; /// Returns local retract coordinates in neighborhood around current rotation - Vector localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; + Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; /// @} /// @name Lie Group @@ -268,7 +271,7 @@ namespace gtsam { /** * Log map at identity - return the canonical coordinates of this rotation */ - static Vector Logmap(const Rot3& R); + static Vector3 Logmap(const Rot3& R); /// @} /// @name Group Action on Point3 @@ -294,10 +297,10 @@ namespace gtsam { /// @{ /** return 3*3 rotation matrix */ - Matrix matrix() const; + Matrix3 matrix() const; /** return 3*3 transpose (inverse) rotation matrix */ - Matrix transpose() const; + Matrix3 transpose() const; /** returns column vector specified by index */ Point3 column(int index) const; @@ -309,19 +312,19 @@ namespace gtsam { * Use RQ to calculate xyz angle representation * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) */ - Vector xyz() const; + Vector3 xyz() const; /** * Use RQ to calculate yaw-pitch-roll angle representation * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) */ - Vector ypr() const; + Vector3 ypr() const; /** * Use RQ to calculate roll-pitch-yaw angle representation * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) */ - Vector rpy() const; + Vector3 rpy() const; /** * Accessor to get to component of angle representations @@ -365,9 +368,18 @@ namespace gtsam { ar & boost::serialization::make_nvp("Rot3", boost::serialization::base_object(*this)); #ifndef GTSAM_DEFAULT_QUATERNIONS - ar & BOOST_SERIALIZATION_NVP(r1_); - ar & BOOST_SERIALIZATION_NVP(r2_); - ar & BOOST_SERIALIZATION_NVP(r3_); + ar & boost::serialization::make_nvp("rot11", rot_(0,0)); + ar & boost::serialization::make_nvp("rot12", rot_(0,1)); + ar & boost::serialization::make_nvp("rot13", rot_(0,2)); + ar & boost::serialization::make_nvp("rot21", rot_(1,0)); + ar & boost::serialization::make_nvp("rot22", rot_(1,1)); + ar & boost::serialization::make_nvp("rot23", rot_(1,2)); + ar & boost::serialization::make_nvp("rot31", rot_(2,0)); + ar & boost::serialization::make_nvp("rot32", rot_(2,1)); + ar & boost::serialization::make_nvp("rot33", rot_(2,2)); +// ar & BOOST_SERIALIZATION_NVP(r1_); +// ar & BOOST_SERIALIZATION_NVP(r2_); +// ar & BOOST_SERIALIZATION_NVP(r3_); #else ar & boost::serialization::make_nvp("w", quaternion_.w()); ar & boost::serialization::make_nvp("x", quaternion_.x()); @@ -389,5 +401,5 @@ namespace gtsam { * @return an upper triangular matrix R * @return a vector [thetax, thetay, thetaz] in radians. */ - std::pair RQ(const Matrix& A); + std::pair RQ(const Matrix& A); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index be7c8c196..b698a77a5 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -28,42 +28,39 @@ using namespace std; namespace gtsam { -static const Matrix I3 = eye(3); +static const Matrix3 I3 = Matrix3::Identity(); /* ************************************************************************* */ -Rot3::Rot3() : - r1_(Point3(1.0,0.0,0.0)), - r2_(Point3(0.0,1.0,0.0)), - r3_(Point3(0.0,0.0,1.0)) {} +Rot3::Rot3() : rot_(Matrix3::Identity()) {} /* ************************************************************************* */ -Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : - r1_(r1), r2_(r2), r3_(r3) {} +Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) { + rot_.col(0) = r1.vector(); + rot_.col(1) = r2.vector(); + rot_.col(2) = r3.vector(); +} /* ************************************************************************* */ Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, - double R31, double R32, double R33) : - r1_(Point3(R11, R21, R31)), - r2_(Point3(R12, R22, R32)), - r3_(Point3(R13, R23, R33)) {} + double R31, double R32, double R33) { + rot_ << R11, R12, R13, + R21, R22, R23, + R31, R32, R33; +} /* ************************************************************************* */ Rot3::Rot3(const Matrix& R) { if (R.rows()!=3 || R.cols()!=3) throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - r1_ = Point3(R(0, 0), R(1, 0), R(2, 0)); - r2_ = Point3(R(0, 1), R(1, 1), R(2, 1)); - r3_ = Point3(R(0, 2), R(1, 2), R(2, 2)); + rot_ = R; } +///* ************************************************************************* */ +//Rot3::Rot3(const Matrix3& R) : rot_(R) {} + /* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) { - Eigen::Matrix3d R = q.toRotationMatrix(); - r1_ = Point3(R.col(0)); - r2_ = Point3(R.col(1)); - r3_ = Point3(R.col(2)); -} +Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { @@ -164,46 +161,39 @@ Point3 Rot3::operator*(const Point3& p) const { return rotate(p); } /* ************************************************************************* */ Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -matrix(); - return Rot3( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); + if (H1) *H1 = -rot_; + return Rot3(rot_.transpose()); } /* ************************************************************************* */ Rot3 Rot3::between (const Rot3& R2, boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); + if (H1) *H1 = -(R2.transpose()*rot_); if (H2) *H2 = I3; - return between_default(*this, R2); + return Rot3(rot_.transpose()*R2.rot_); + //return between_default(*this, R2); } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); + return Rot3(rot_*R2.rot_); } /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, boost::optional H1, boost::optional H2) const { if (H1 || H2) { - const Matrix R(matrix()); - if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = R; + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; } - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); + return Point3(rot_ * p.vector()); } /* ************************************************************************* */ // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - Point3 q( - r1_.x()*p.x()+r1_.y()*p.y()+r1_.z()*p.z(), - r2_.x()*p.x()+r2_.y()*p.y()+r2_.z()*p.z(), - r3_.x()*p.x()+r3_.y()*p.y()+r3_.z()*p.z() - ); // q = Rt*p + Point3 q(rot_.transpose()*p.vector()); // q = Rt*p if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); if (H2) *H2 = transpose(); return q; @@ -211,25 +201,26 @@ Point3 Rot3::unrotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector Rot3::Logmap(const Rot3& R) { +Vector3 Rot3::Logmap(const Rot3& R) { static const double PI = boost::math::constants::pi(); + const Matrix3& rot = R.rot_; // Get trace(R) - double tr = R.r1_.x()+R.r2_.y()+R.r3_.z(); + double tr = rot.trace(); // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (std::abs(tr+1.0) < 1e-10) { - if(std::abs(R.r3_.z()+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*R.r3_.z())) * - Vector_(3, R.r3_.x(), R.r3_.y(), 1.0+R.r3_.z()); - else if(std::abs(R.r2_.y()+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*R.r2_.y())) * - Vector_(3, R.r2_.x(), 1.0+R.r2_.y(), R.r2_.z()); + if(std::abs(rot(2,2)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(2,2) )) * + Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); + else if(std::abs(rot(1,1)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(1,1))) * + Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*R.r1_.x())) * - Vector_(3, 1.0+R.r1_.x(), R.r1_.y(), R.r1_.z()); + return (PI / sqrt(2.0+2.0*rot(0,0))) * + Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); } else { double magnitude; double tr_3 = tr-3.0; // always negative @@ -241,10 +232,10 @@ Vector Rot3::Logmap(const Rot3& R) { // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) magnitude = 0.5 - tr_3*tr_3/12.0; } - return magnitude*Vector_(3, - R.r2_.z()-R.r3_.y(), - R.r3_.x()-R.r1_.z(), - R.r1_.y()-R.r2_.x()); + return magnitude*Vector3( + rot(2,1)-rot(1,2), + rot(0,2)-rot(2,0), + rot(1,0)-rot(0,1)); } } @@ -276,7 +267,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { } /* ************************************************************************* */ -Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { +Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { if(mode == Rot3::EXPMAP) { return Logmap(between(T)); } else if(mode == Rot3::CAYLEY) { @@ -292,13 +283,13 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { const double x = (a * f - cd + f) * K; const double y = (b * f - ce - c) * K; const double z = (fg - di - d) * K; - return -2 * Vector_(3, x, y, z); + return -2 * Vector3(x, y, z); } else if(mode == Rot3::SLOW_CAYLEY) { // Create a fixed-size matrix Eigen::Matrix3d A(between(T).matrix()); // using templated version of Cayley Matrix Omega = Cayley<3>(A); - return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); + return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); } else { assert(false); exit(1); @@ -306,89 +297,69 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { } /* ************************************************************************* */ -Matrix Rot3::matrix() const { - Matrix R(3,3); - R << - r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z(); - return R; +Matrix3 Rot3::matrix() const { + return rot_; } /* ************************************************************************* */ -Matrix Rot3::transpose() const { - Matrix Rt(3,3); - Rt << - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z(); - return Rt; +Matrix3 Rot3::transpose() const { + return rot_.transpose(); } /* ************************************************************************* */ Point3 Rot3::column(int index) const{ - if(index == 3) - return r3_; - else if(index == 2) - return r2_; - else if(index == 1) - return r1_; // default returns r1 - else - throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); + return Point3(rot_.col(index)); } /* ************************************************************************* */ -Point3 Rot3::r1() const { return r1_; } +Point3 Rot3::r1() const { return Point3(rot_.col(0)); } /* ************************************************************************* */ -Point3 Rot3::r2() const { return r2_; } +Point3 Rot3::r2() const { return Point3(rot_.col(1)); } /* ************************************************************************* */ -Point3 Rot3::r3() const { return r3_; } +Point3 Rot3::r3() const { return Point3(rot_.col(2)); } /* ************************************************************************* */ -Vector Rot3::xyz() const { - Matrix I;Vector q; +Vector3 Rot3::xyz() const { + Matrix I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } /* ************************************************************************* */ -Vector Rot3::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); +Vector3 Rot3::ypr() const { + Vector3 q = xyz(); + return Vector3(q(2),q(1),q(0)); } /* ************************************************************************* */ -Vector Rot3::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); +Vector3 Rot3::rpy() const { + Vector3 q = xyz(); + return Vector3(q(0),q(1),q(2)); } /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { - return Quaternion((Eigen::Matrix3d() << - r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z()).finished()); + return Quaternion(rot_); } /* ************************************************************************* */ -pair RQ(const Matrix& A) { +pair RQ(const Matrix& A) { double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); - Matrix B = A * Qx.matrix(); + Matrix3 B = A * Qx.matrix(); double y = -atan2(B(2, 0), B(2, 2)); Rot3 Qy = Rot3::Ry(-y); - Matrix C = B * Qy.matrix(); + Matrix3 C = B * Qy.matrix(); double z = -atan2(-C(1, 0), C(1, 1)); Rot3 Qz = Rot3::Rz(-z); - Matrix R = C * Qz.matrix(); + Matrix3 R = C * Qz.matrix(); - Vector xyz = Vector_(3, x, y, z); + Vector xyz = Vector3(x, y, z); return make_pair(R, xyz); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d86c37a30..c67834be3 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -49,6 +49,10 @@ namespace gtsam { Rot3::Rot3(const Matrix& R) : quaternion_(Eigen::Matrix3d(R)) {} +// /* ************************************************************************* */ +// Rot3::Rot3(const Matrix3& R) : +// quaternion_(R) {} + /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} @@ -140,7 +144,7 @@ namespace gtsam { /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3::Logmap(const Rot3& R) { + Vector3 Rot3::Logmap(const Rot3& R) { Eigen::AngleAxisd angleAxis(R.quaternion_); if(angleAxis.angle() > M_PI) // Important: use the smallest possible angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep @@ -155,15 +159,15 @@ namespace gtsam { } /* ************************************************************************* */ - Vector Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const { + Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const { return Logmap(between(t2)); } /* ************************************************************************* */ - Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); } + Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); } /* ************************************************************************* */ - Matrix Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); } + Matrix3 Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); } /* ************************************************************************* */ Point3 Rot3::column(int index) const{ @@ -187,29 +191,29 @@ namespace gtsam { Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } /* ************************************************************************* */ - Vector Rot3::xyz() const { - Matrix I;Vector q; + Vector3 Rot3::xyz() const { + Matrix I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } /* ************************************************************************* */ - Vector Rot3::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); + Vector3 Rot3::ypr() const { + Vector3 q = xyz(); + return Vector3(q(2),q(1),q(0)); } /* ************************************************************************* */ - Vector Rot3::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); + Vector3 Rot3::rpy() const { + Vector3 q = xyz(); + return Vector3(q(0),q(1),q(2)); } /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ - pair RQ(const Matrix& A) { + pair RQ(const Matrix& A) { double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); @@ -223,7 +227,7 @@ namespace gtsam { Rot3 Qz = Rot3::Rz(-z); Matrix R = C * Qz.matrix(); - Vector xyz = Vector_(3, x, y, z); + Vector xyz = Vector3(x, y, z); return make_pair(R, xyz); }