/** * @brief Convert euler angles to quaternion. */ Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy);

这是一个函数，用于将欧拉角转换为四元数。输入参数rpy是一个包含三个欧拉角的向量，返回值是一个四元数。具体实现可能如下所示：

Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy) {
    double roll = rpy.x();
    double pitch = rpy.y();
    double yaw = rpy.z();
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q;
}

这里使用Eigen库实现了欧拉角转换为四元数的功能。具体来说，先将欧拉角分别转换为绕x轴、y轴、z轴旋转的角轴，然后按照z-y-x的顺序组合起来，得到一个总的绕x、y、z轴旋转的角轴，最后将这个角轴转换为四元数。