From 3c1ddd7a3fea0027e752eb87495bb3ea25e9c366 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 19:22:14 -0800 Subject: [PATCH 1/3] Inlined skewSymmetric --- gtsam/base/Matrix.cpp | 9 --------- gtsam/base/Matrix.h | 12 +++++++++--- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3cafdd0ba..c6af89486 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -580,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) { return M; } -/* ************************************************************************* */ -Matrix3 skewSymmetric(double wx, double wy, double wz) -{ - return (Matrix3() << - 0.0, -wz, +wy, - +wz, 0.0, -wx, - -wy, +wx, 0.0).finished(); -} - /* ************************************************************************* */ Matrix LLt(const Matrix& A) { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 41ffa27b5..e2b40b02b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -477,9 +477,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask * @param wz * @return a 3*3 skew symmetric matrix */ -GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz); -template -inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} + +inline Matrix3 skewSymmetric(double wx, double wy, double wz) { + return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished(); +} + +template +inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { + return skewSymmetric(w(0), w(1), w(2)); +} /** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); From c1b2e9d72654b8b7800dd8ad0c905772f5008104 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 19:22:49 -0800 Subject: [PATCH 2/3] Optmized ExpmapDerivative --- gtsam/geometry/SO3.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index af5803cb7..a417164e4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -133,9 +133,9 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { using std::cos; using std::sin; - double theta2 = omega.dot(omega); + const double theta2 = omega.dot(omega); if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; - double theta = std::sqrt(theta2); // rotation angle + const double theta = std::sqrt(theta2); // rotation angle #ifdef DUY_VERSION /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) Matrix3 X = skewSymmetric(omega); @@ -154,9 +154,15 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { * a perturbation on the manifold (expmap(Jr * omega)) */ // element of Lie algebra so(3): X = omega^, normalized by normx - const Matrix3 Y = skewSymmetric(omega) / theta; - return I_3x3 - ((1 - cos(theta)) / (theta)) * Y - + (1 - sin(theta) / theta) * Y * Y; // right Jacobian + const double wx = omega.x(), wy = omega.y(), wz = omega.z(); + Matrix3 Y; + Y << 0.0, -wz, +wy, + +wz, 0.0, -wx, + -wy, +wx, 0.0; + const double s2 = sin(theta / 2.0); + const double a = (2.0 * s2 * s2 / theta2); + const double b = (1.0 - sin(theta) / theta) / theta2; + return I_3x3 - a * Y + b * Y * Y; #endif } From a0f32f6d1421f854d044ece939181243e9c346af Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 19:23:18 -0800 Subject: [PATCH 3/3] Got rid of dynamic Matrix in rotate --- gtsam/geometry/Rot3Q.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index b255b082d..2497f6806 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -27,14 +27,12 @@ using namespace std; namespace gtsam { - static const Matrix I3 = eye(3); - /* ************************************************************************* */ Rot3::Rot3() : quaternion_(Quaternion::Identity()) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << col1.x(), col2.x(), col3.x(), col1.y(), col2.y(), col3.y(), col1.z(), col2.z(), col3.z()).finished()) {} @@ -43,7 +41,7 @@ namespace gtsam { Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << R11, R12, R13, R21, R22, R23, R31, R32, R33).finished()) {} @@ -91,10 +89,10 @@ namespace gtsam { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - Matrix R = matrix(); + const Matrix3 R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; - Eigen::Vector3d r = R * p.vector(); + const Vector3 r = R * p.vector(); return Point3(r.x(), r.y(), r.z()); }