Eigen::Matrix<typename Derived::Scalar, 4, 1> quat2axis(const Eigen::MatrixBase<Derived>& q)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
auto q_normalized = q.normalized();
auto s = std::sqrt(1.0 - q_normalized(0) * q_normalized(0)) + std::numeric_limits<typename Derived::Scalar>::epsilon();
Eigen::Matrix<typename Derived::Scalar, 4, 1> a;
a << q_normalized.template tail<3>() / s, 2.0 * std::acos(q_normalized(0));
return a;
}
Eigen::Matrix<typename Derived::Scalar, 3, 1> quat2rpy(const Eigen::MatrixBase<Derived>& q)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
auto q_normalized = q.normalized();
auto w = q_normalized(0);
auto x = q_normalized(1);
auto y = q_normalized(2);
auto z = q_normalized(3);
Eigen::Matrix<typename Derived::Scalar, 3, 1> ret;
ret << std::atan2(2.0*(w*x + y*z), w*w + z*z -(x*x +y*y)),
std::asin(2.0*(w*y - z*x)),
std::atan2(2.0*(w*z + x*y), w*w + x*x-(y*y+z*z));
return ret;
}
Eigen::Matrix<typename Derived::Scalar, 3, 3> quat2rotmat(const Eigen::MatrixBase<Derived>& q)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
auto q_normalized = q.normalized();
auto w = q_normalized(0);
auto x = q_normalized(1);
auto y = q_normalized(2);
auto z = q_normalized(3);
Eigen::Matrix<typename Derived::Scalar, 3, 3> M;
M.row(0) << w * w + x * x - y * y - z * z, 2.0 * x * y - 2.0 * w * z, 2.0 * x * z + 2.0 * w * y;
M.row(1) << 2.0 * x * y + 2.0 * w * z, w * w + y * y - x * x - z * z, 2.0 * y * z - 2.0 * w * x;
M.row(2) << 2.0 * x * z - 2.0 * w * y, 2.0 * y * z + 2.0 * w * x, w * w + z * z - x * x - y * y;
return M;
}