Eigen::Matrix<typename Derived::Scalar, 4, 1> rotmat2axis(const Eigen::MatrixBase<Derived>& R) { EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3); typename Derived::Scalar theta = std::acos((R.trace() - 1.0) / 2.0); Vector4d a; if (theta > std::numeric_limits<typename Derived::Scalar>::epsilon()) { a << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1), theta; a.head<3>() *= 1.0 / (2.0 * std::sin(theta)); } else { a << 1.0, 0.0, 0.0, 0.0; } return a; }