/** * @brief Store Quaternion to MAVLink float[4] format * * MAVLink uses wxyz order, wile Eigen::Quaterniond uses xyzw internal order, * so it can't be stored to array using Eigen::Map. */ template <typename _Scalar, typename std::enable_if<std::is_floating_point<_Scalar>::value, bool>::type = true> inline void quaternion_to_mavlink(const Eigen::Quaternion<_Scalar> &q, std::array<float, 4> &qmsg) { qmsg[0] = q.w(); qmsg[1] = q.x(); qmsg[2] = q.y(); qmsg[3] = q.z(); }
时间: 2023-06-19 19:07:36 浏览: 180
这是一个C++模板函数,用于将Eigen库中的四元数转换为MAVLink协议中的四元数数组格式,其中MAVLink协议使用wxyz顺序,而Eigen库中的Quaterniond类使用xyzw内部顺序。该函数使用std::array作为MAVLink四元数数组的容器,并将四元数的四个分量依次存储到容器中。函数的模板参数_Scalar表示四元数分量的类型,通常为float或double类型。需要注意的是,该函数只适用于实数类型的四元数。
相关问题
/** * @brief Convert quaternion to euler angles * * Reverse operation to @a quaternion_from_rpy() */ Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q);
这是一个将四元数转换为欧拉角的函数,其中Eigen::Quaterniond是Eigen库中的四元数类型,Eigen::Vector3d是Eigen库中的三维向量类型。欧拉角是指描述物体在三维空间中的旋转时,分别绕着三个轴线旋转的角度。在这里,将四元数转换为欧拉角可以用于将旋转信息从四元数形式转换为更直观的欧拉角形式,方便人类理解。
/** * @brief Convert Mavlink float[4] quaternion to Eigen */ inline Eigen::Quaterniond mavlink_to_quaternion(const std::array<float, 4> &q) { return Eigen::Quaterniond(q[0], q[1], q[2], q[3]); }
这是一个 C++ 的 inline 函数,用于将 Mavlink 协议中表示四元数的 float 数组转换为 Eigen 库中的 Quaterniond 类型。
具体来说,函数的参数是一个包含四个 float 值的数组 q,依次表示四元数的实部和三个虚部。函数通过 Eigen 库中的 Quaterniond 类型来表示四元数,并返回转换后的 Quaterniond 对象。
函数的实现很简单,直接调用了 Eigen 库中的 Quaterniond 构造函数,将数组 q 中的四个值作为构造函数的参数传入。由于该函数被声明为 inline,因此在编译时会直接将函数的代码插入到调用该函数的地方,从而避免了函数调用的开销。
阅读全文
相关推荐
















