Reimplement log_R()

src/rotations.h

@@ -193,6 +193,19 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBa
         return Eigen::Matrix<T, 3, 3>::Identity() + skew(_v);
 }
 
+/** \brief rotation matrix to quaternion conversion
+ *
+ * @param _R a rotation matrix
+ * @return the equivalent right-handed unit quaternion
+ */
+template<typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar> R2q(const Eigen::MatrixBase<Derived>& _R)
+{
+    MatrixSizeCheck<3,3>::check(_R);
+
+    return Eigen::Quaternion<typename Derived::Scalar>(_R);
+}
+
 /** \brief Rotation matrix logarithmic map
  *
  * @param _R a 3D rotation matrix
@@ -201,12 +214,7 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBa
 template<typename Derived>
 inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBase<Derived>& _R)
 {
-    MatrixSizeCheck<3, 3>::check(_R);
-
-    typedef typename Derived::Scalar T;
-
-    Eigen::AngleAxis<T> aa = Eigen::AngleAxis<T>(_R);
-    return aa.axis() * aa.angle();
+    return log_q(R2q(_R));
 }
 
 /** \brief Rotation vector to quaternion conversion
@@ -277,19 +285,6 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase
     return q2R( q );
 }
 
-/** \brief rotation matrix to quaternion conversion
- *
- * @param _R a rotation matrix
- * @return the equivalent right-handed unit quaternion
- */
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar> R2q(const Eigen::MatrixBase<Derived>& _R)
-{
-    MatrixSizeCheck<3,3>::check(_R);
-
-    return Eigen::Quaternion<typename Derived::Scalar>(_R);
-}
-
 /////////////////////////////////////////////////////////////////
 // Jacobians of SO(3)