点云的主轴方向错误

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();

这将产生以下可视化结果：

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

我觉得小箭头要么向上，要么向下。
箭头方向的值orientation.w是什么意思？

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

Answer 1

方向由 tf 包中的 Quaternions in ROS, not by directional vectors. Quaternions can be a bit unintuitive, but fortunately there are some 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 设置为 < 1 的某个值以减少箭头的长度。

点云的主轴方向错误

Bad Orientation of Principal Axis of a Point Cloud

axis

pca

point-cloud-library

ros

point-clouds