rpy专题

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