utils.h

#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

inline Eigen::Vector3f RotationQuaternionToEulerVector(const Eigen::Quaternionf& quaternion)
{
  Eigen::Quaternionf n_q = quaternion.normalized();
  // We choose the quaternion with positive 'w', i.e., the one with a smaller
  // angle that represents this orientation.
  if (n_q.w() < 0.) { 
    n_q.w() = -1. * n_q.w();
    n_q.x() = -1. * n_q.x();
    n_q.y() = -1. * n_q.y();
    n_q.z() = -1. * n_q.z();
  }
  // We convert the normalized_quaternion(n_q) into a vector along the rotation
  // axis with length of the rotation angle.
  float  roll = atan2(2 * (n_q.w() * n_q.x() + n_q.y() * n_q.z()),
                       1 - 2 * (n_q.x() * n_q.x() + n_q.y() * n_q.y()));
  float  pitch = asin(2 * (n_q.w() * n_q.y() - n_q.z() * n_q.x()));
  float  yaw = atan2(2 * (n_q.w() * n_q.z() + n_q.x() * n_q.y()),
                      1 - 2 * (n_q.y() * n_q.y() + n_q.z() * n_q.z()));
  Eigen:: Vector3f atti(roll, pitch, yaw);                   
  return atti;
}