ROS RVIZ：如何以正确的方向可视化 PCL 模型？

Question

我正在尝试使用 PCL RANSAC 函数可视化适合点云的圆柱体。当我拟合模型时，我得到一个

pcl::ModelCoefficients

对象，它具有以下系数 point_on_axis、axis_direction、cylinder_radius_R，请参阅此处的文档。

据我了解，轴方向值（系数对象值成员的值3、4和5）分别是圆柱轴向量的x、y和z分量。要使用标记对象在 RVIZ 中显示此向量，我需要将此向量转换为四元数。我一直在使用以下 C++ 代码执行此操作：

        //Convert axis vector to quarternion format
        double axis_pitch = atan2(coefficients_cylinder.values[5],coefficients_cylinder.values[4]);
        double axis_roll = atan2(coefficients_cylinder.values[3],coefficients_cylinder.values[5]);
        double axis_yaw = atan2(coefficients_cylinder.values[3],coefficients_cylinder.values[4]);
        tf2::Quaternion axis_quarternion;
        axis_quarternion.setRPY( axis_roll, axis_pitch, axis_yaw );
        axis_quarternion.normalize();

但是，每当我查看覆盖在原始点云上的圆柱体标记时，它们的方向都是错误的。

这是什么原因造成的？我是否错过了转换过程中的一个步骤，或者我的方法完全偏离了？

感谢您的帮助！

Answer 1

我认为一个问题是 PCL 使用光轴框架（右手定义：x=down，y=left，z=forward）。

这就是为什么我发现能够表示此方向的最佳方法是从 Eigen::AngleAxis 获得 Eigen::QuaternionX，给出角度和测量的轴。

尝试使用此函数将 RANSAC 圆柱模型转换为 geometry_msgs/Quaternion。此方向将用于在 Rviz 中显示圆柱体标记。

#include <tf2_eigen/tf2_eigen.h>

inline Eigen::Quaterniond obtainCylinderOrientationFromModel(
        const pcl::ModelCoefficients::ConstPtr& coefficients)
{
    Eigen::Vector3d axis_vector(coefficients->values[3],
                                coefficients->values[4],
                                coefficients->values[5]);
    Eigen::Vector3d up_vector(0.0, 0.0, -1.0);
    Eigen::Vector3d right_vector = axis_vector.cross(up_vector);
    right_vector.normalized();

    Eigen::Quaterniond q(Eigen::AngleAxisd(-1.0 * std::acos(axis_vector.dot(up_vector)), 
                                           right_vector));
    q.normalize();

    return q;
}

...

// Obtain geometry_msgs/Quaternion from model coefficients
pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients);
const auto q = obtainCylinderOrientationFromModel(coeffs);
geometry_msgs::Quaternion cyl_q =      
      tf2::toMsg(obtainCylinderOrientationFromDirection(q));

希望对您有所帮助！