点云主轴的不良方向

Question

我试图通过主成分分析来计算主轴。我有一个pointcloud并使用Point Cloud Library（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来生成四元数，例如，从滚动/俯仰/偏航角度。

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

在您的特殊情况下，有一个更简单的解决方案：除了设置箭头的原点和方向之外，还可以定义起点和终点（参见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设置为某个值＆lt; 1减少箭头的长度。