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

主要使用Eigen库实现：

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

#include 
#include 

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

  Eigen::Matrix q = q_in.normalized().coeffs();

  Scalar s = -2 * (q(0) * q(2) - q(3) * q(1));
  if (s > 1)
    s = 1;
  return (Eigen::Matrix()
              << 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  vec;
 vec.