Merged in feature/small_Rot3_optimizations (pull request #203)

Feature/small_rot3_optimizations

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)
 {

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<class Derived>
-inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& 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 <class Derived>
+inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& 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);

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());
   }
 

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<double>::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
 }