我试图可视化一个圆柱体,我使用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();
但是,每当我查看覆盖在原始点云上的圆柱体标记时,它们的方向都是错误的。
是什么导致了这种情况?是我在转换过程中遗漏了一个步骤,还是我的方法完全错误?
谢谢你的帮助!
1条答案
按热度按时间2w2cym1i1#
我认为一个问题是PCL使用光轴框架(右手定义:x=向下,y=向左,z=向前)。
这就是为什么我发现能够表示这个方向的最佳方法是从Eigen::AngleAxis获得Eigen::QuaternionX,给出Angular 和测量Angular 的轴。
尝试使用此函数从RANSAC圆柱体模型转换为geometry_msgs/Quaternion。此方向将用于在Rviz中显示圆柱体标记。
我希望这对你有帮助!