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

fgw7neuy 于 2023-04-01 发布在其他

关注(0)|答案(1)|浏览(250)

我试图可视化一个圆柱体，我使用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();

但是，每当我查看覆盖在原始点云上的圆柱体标记时，它们的方向都是错误的。
是什么导致了这种情况？是我在转换过程中遗漏了一个步骤，还是我的方法完全错误？
谢谢你的帮助！

c++

来源：https://stackoverflow.com/questions/75687348/ros-rviz-how-to-visualize-pcl-model-with-correct-orientation

1条答案

按热度按时间

2w2cym1i1#

我认为一个问题是PCL使用光轴框架（右手定义：x=向下，y=向左，z=向前）。
这就是为什么我发现能够表示这个方向的最佳方法是从Eigen：：AngleAxis获得Eigen：：QuaternionX，给出Angular 和测量Angular 的轴。
尝试使用此函数从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));

我希望这对你有帮助！

赞(0）回复(0）举报 2023-04-01

我来回答

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

1条答案

相关问题

热门标签

最新问答