求两个(法)向量之间的rpy夹角

主要使用Eigen库实现：

1. 四元素到欧拉角的转换

#include <array>
#include <Eigen/Geometry>

template <typename T>
inline Eigen::Matrix<typename std::remove_reference<T>::type::Scalar, 3, 1> eulerAnglesZYX(T q_in)
{
  typedef typename std::remove_reference<T>::type::Scalar Scalar;

  Eigen::Matrix<Scalar, 4, 1> q = q_in.normalized().coeffs();

  Scalar s = -2 * (q(0) * q(2) - q(3) * q(1));
  if (s > 1)
    s = 1;
  return (Eigen::Matrix<Scalar, 3, 1>()
              << atan2f(2 * (q(0) * q(1) + q(3) * q(2)), q(3) * q(3) + q(0) * q(0) - q(1) * q(1) - q(2) * q(2)),
          asin(s), atan2(2 * (q(1) * q(2) + q(3) * q(0)), q(3) * q(3) - q(0) * q(0) - q(1) * q(1) + q(2) * q(2)))
      .finished();
};

2. 代码演示

int main(int argc, char** argv)
{
  Eigen::Vector3d total_normal(0.999999,0.001126,0.000244);

 std::vector<Eigen::Vector3d>  vec;
 vec.