欧拉到四元数的转换 PCL + Eigen

Question

我正在使用点云库并尝试将两个点云匹配在一起 ICP（迭代最近点）算法。我得到的数据集带有来自 IMU 传感器的 X Y Z 方向值。

我将这些附加到点云对象的 sensor_orientation_ 属性以帮助匹配过程。查看 PCL 文档，它被指定为：传感器采集姿势（云数据坐标系中的旋转）。注：数据以(w, x, y, z)格式存储。

所以，为了从 IMU 数据转换我正在使用下面的这个函数，我想问一下它是否正确？

Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;

    return q;
}

然后方法被调用：

inCloud->sensor_orientation_ = Eigen::Quaternionf(euler2Quaternion(orientX, orientY, orientZ));

Answer 1

好吧，我发现了代码的问题。我在官方文档中找不到的是 Eigen::AngleAxisd 将输入作为 radians而不是 degrees，所以一旦您将数据从 IMU 传感器转换为弧度，一切似乎都很好:)

函数现在看起来像这样：

Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
        Eigen::AngleAxisd rollAngle((roll*M_PI) / 180, Eigen::Vector3d::UnitZ());
        Eigen::AngleAxisd yawAngle((yaw*M_PI) / 180, Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd pitchAngle((pitch*M_PI) / 180, Eigen::Vector3d::UnitX());

        Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
        return q;
}

显然，俯仰、偏航和滚动的使用顺序取决于用法，但在我的例子中，这是有效的:)

欧拉到四元数的转换 PCL + Eigen

Euler to quaternion conversion PCL + Eigen

c++

math

rotation

eigen

point-cloud-library