Bad Orientation of Principal Axis of a Point Cloud

Question

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. I think the small arrow should go either up, or downwards.
2. What does the value orientation.w of the arrow's orientation mean?

Do you have some hints what I did wrong?

Answer 1

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

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