点云主轴方向错误

Question

我正在尝试通过主成分分析来计算主轴。我有一个点云并为此使用点云库（pcl）。此外，我尝试用标记可视化我在 rviz 中计算的主轴。这是我使用的代码片段：

void computePrincipalAxis(const PointCloud& cloud, Eigen::Vector4f& centroid, Eigen::Matrix3f& evecs, Eigen::Vector3f& evals) {
  Eigen::Matrix3f covariance_matrix;
  pcl::computeCovarianceMatrix(cloud, centroid, covariance_matrix);
  pcl::eigen33(covariance_matrix, evecs, evals);
}

void createArrowMarker(Eigen::Vector3f& vec, int id, double length) {
  visualization_msgs::Marker marker;
  marker.header.frame_id = frameId;
  marker.header.stamp = ros::Time();
  marker.id = id;
  marker.type = visualization_msgs::Marker::ARROW;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];
  marker.pose.orientation.x = vec[0];
  marker.pose.orientation.y = vec[1];
  marker.pose.orientation.z = vec[2];
  marker.pose.orientation.w = 1.0;
  marker.scale.x = length;
  marker.scale.y = 0.02;
  marker.scale.z = 0.02;
  marker.color.a = 1.0;
  marker.color.r = 1.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  featureVis.markers.push_back(marker);
}

Eigen::Vector4f centroid;
Eigen::Matrix3f evecs;
Eigen::Vector3f evals;

// Table is the pointcloud of the table only.
pcl::compute3DCentroid(*table, centroid); 
computePrincipalAxis(*table, centroid, evecs, evals);

Eigen::Vector3f vec;

vec << evecs.col(0);
createArrowMarker(vec, 1, evals[0]);

vec << evecs.col(1);
createArrowMarker(vec, 2, evals[1]);

vec << evecs.col(2);
createArrowMarker(vec, 3, evals[2]);

publish();

这会产生以下可视化效果：

我知道规模不是很完美。两个较长的箭头太长了。但我对一些事情感到困惑：

1. 我认为小箭头应该向上或向下。
2. 箭头方向的值orientation.w是什么意思？

你有什么提示我做错了什么吗？

Answer 1

方向在 ROS 中由Quaternions 表示，而不是由方向向量表示。四元数可能有点不直观，但幸运的是，tf 包中有一些 helper functions，用于生成四元数，例如从 roll/pitch/yaw-angles。

因此，修复标记的一种方法是将方向向量转换为四元数。

在您的特殊情况下，有一个更简单的解决方案：除了设置箭头的原点和方向，还可以定义起点和终点（请参阅ROS wiki about marker types）。所以不用设置pose属性，只需在points属性中添加起点和终点：

float k = 1.0; // optional to scale the length of the arrows

geometry_msgs::Point p;
p.x = centroid[0];
p.y = centroid[1];
p.z = centroid[2];
marker.points.push_back(p);
p.x += k * vec[0];
p.y += k * vec[1];
p.z += k * vec[2];
marker.points.push_back(p);

您可以将k 设置为某个值