forre.st

eigenfuncs

Quaternion to rotation vector and other miscellaneous useful functions using the Eigen math library

#include <boost/math/special_functions/sinc.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>

Eigen::Quaterniond quat_from_rotvec(Eigen::Vector3d r) {
    double angle = r.norm();
    Eigen::Quaterniond res;
    res.w() = cos(angle/2);
    res.vec() = boost::math::sinc_pi(angle/2)/2 * r; // = sin(angle/2) * r.normalized(), without the singularity
    return res;
}

Eigen::Vector3d rotvec_from_quat(Eigen::Quaterniond q) {
  q = q.normalized();
  if(q.w() < 0) q = Eigen::Quaterniond(-q.coeffs());
  return 2/boost::math::sinc_pi(acos(std::min(1., q.w()))) * q.vec();
}


template<int N>
Eigen::Matrix<double, N, N> cholesky(Eigen::Matrix<double, N, N> x) {
  Eigen::LDLT<Eigen::Matrix<double, N, N> > ldlt = x.ldlt();
  return ldlt.transpositionsP().transpose() * Eigen::Matrix<double, N, N>(ldlt.matrixL()) * Eigen::Matrix<double, N, 1>(ldlt.vectorD().array().sqrt()).asDiagonal();
}

The cholesky function uses Eigen's LDLT decomposition so it can handle positive semidefinite matrices.