ROS RVIZ：如何正确可视化PCL拟合的圆柱模型？

我尝试在ROS RVIZ中可视化通过PCL RANSAC算法拟合点云得到的圆柱模型。拟合后得到pcl::ModelCoefficients对象，包含轴上点、轴方向向量、圆柱半径（参考官方文档）。我认为系数数组的3、4、5位分别是轴方向的x、y、z分量，需要将其转换为四元数以在RVIZ中通过Marker显示，初始使用如下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();

但叠加在原点云上的圆柱Marker始终姿态错误。请问问题原因是什么？是转换步骤缺失还是方法错误？

我找到可行的修正代码：

//Convert axis vector to quarternion format
double axis_roll = atan2(coefficients_cylinder->values[5],coefficients_cylinder->values[4]);
double axis_pitch = -1.0 * atan2(coefficients_cylinder->values[5],coefficients_cylinder->values[3]);
double axis_yaw = atan2(coefficients_cylinder->values[4],coefficients_cylinder->values[3]);
tf2::Quaternion axis_quarternion;
axis_quarternion.setRPY( 0.0, -0.5*M_PI + axis_pitch, axis_yaw);
// axis_quarternion.setRPY( axis_roll, axis_pitch, axis_yaw);
axis_quarternion.normalize();

问题根源在于两点：

• 初始代码中对轴方向向量到欧拉角的转换逻辑错误，欧拉角的计算方式不符合坐标系旋转的正确映射关系；
• 未考虑RVIZ中圆柱Marker默认沿Z轴对齐的特性，需要额外旋转变换将拟合得到的圆柱轴方向映射到Marker坐标系中。

内容的提问来源于stack exchange，提问作者DuffRumkins