求两个(法)向量之间的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.