点云的主轴方向错误 [英] Bad Orientation of Principal Axis of a Point Cloud

问题描述

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

I'm trying to calculate the principal axis via principal component analysis. I have a pointcloud and use for this the Point Cloud Library (pcl). Furthermore, I try to visualize the principal axis I calculated in rviz with markers. Here is the code snipped I use:

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

这将产生以下可视化效果:

This results in the following visualization:

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

I'm aware that the scale is not very perfect. The two longer arrows are much too long. But I'm confused about a few things:

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

您有什么提示我做错了吗?

Do you have some hints what I did wrong?

推荐答案

方向由帮助函数 tf 包，以生成四元数，例如，从滚动/俯仰/偏航角.

Orientations are represented by Quaternions in ROS, not by directional vectors. Quaternions can be a bit unintuitive, but fortunately there are some helper functions in the tf package, to generate quaternions, for example, from roll/pitch/yaw-angles.

因此，一种固定标记的方法是将方向矢量转换为四元数.

One way to fix the marker would therefore be, to convert the direction vector into a quaternion.

在您的特殊情况下，有一个更简单的解决方案:除了设置箭头的原点和方向，还可以定义起点和终点(请参见

In your special case, there is a much simpler solution, though: Instead of setting origin and orientation of the arrow, it is also possible to define start and end point (see ROS wiki about marker types). So instead of setting the pose attribute, just add start and end point to the points attribute:

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以减少箭头的长度.

You can set k to some value < 1 to reduce the length of the arrows.

这篇关于点云的主轴方向错误的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！