Orthogonal Distance Regression Plane for a given PointCloud -- Am I Doing This Correctly?

[Note: At karthik's suggestion, I have also posted this to the PCL-user's list.]

I've written the following method to compute the orthogonal distance regression plane for a given PointCloud, using the method detailed here.

However, visualizing it in rviz has proven to be a bit tricky, probably mostly due to my inexperience with quaternions. What I've got below seems to mostly work, but sometimes the plane (represented by a set of PoseStamped axes oriented along the plane's normal vector -- call this vector n) displayed by rviz seems in some cases to be a bit more off than I would expect, and so I'm still unsure if I'm doing everything properly.

The two major places I'm uncertain about are in my implementation of the fitting algorithm from the above link (eg. am I properly using the Eigen::JacobiSVD and related matrices to calculate the correct things?), and in my calculation of the orientation quaternion.

Here's the idea behind the calculation of the quaternion the way I have it. As from Wikipedia, the quaternion q is defined in relation to rotations by:

q = cos(a/2) + u * sin(a/2)

Where a is the angle to rotate by, and u is the unit vector about which to rotate.

So I assumed the 'starting' vector would be (0,0,1), which should be rotated by q until it is pointing in the same direction as the plane normal. Thus, the rotation axis u is the cross product of (0,0,1) and n, and a is the angle between them, computed as a = arcsine((0,0,1) DOT n).

Thank you!

if (points.size() >= 3) {

Eigen::MatrixXd m(points.size(), 3);

// Compute centroid
double centroid_x = 0;
double centroid_y = 0;
double centroid_z = 0;
BOOST_FOREACH(const pcl::PointXYZ p, points) {
centroid_x += p.x;
centroid_y += p.y;
centroid_z += p.z;
}
centroid_x /= points.size();
centroid_y /= points.size();
centroid_z /= points.size();

// Define m
int i=0;
BOOST_FOREACH(const pcl::PointXYZ p, points) {
m(i,0) = p.x - centroid_x;
m(i,1) = p.y - centroid_y;
m(i,2) = p.z - centroid_z;
i++;
}

// Compute SVD
Eigen::JacobiSVD<Eigen::MatrixXd> svd(m, Eigen::ComputeThinV);

const int last = svd.cols() - 1;
double normal_x = svd.matrixV()(0,last);
double normal_y = svd.matrixV()(1,last);
double normal_z = svd.matrixV()(2,last);

ROS_INFO("Centroid: [%f, %f, %f]", centroid_x, centroid_y, centroid_z);
ROS_INFO("Normal:   [%f, %f, %f]\n", normal_x, normal_y, normal_z);

// Publish the normal for display in rviz
geometry_msgs::PoseStamped normal;
normal.pose.position.x = centroid_x;
normal.pose.position.y = centroid_y;
normal.pose.position.z = centroid_z;

// Yes I realize some of the math here could probably be simplified using
// trig identities, but for now I just want it to work. ;)
normal.pose.orientation.w = std::cos(0.5 * std::asin(normal_z));
normal.pose.orientation.x = -normal_y * std::sin(0.5 * std::asin(normal_z));
normal.pose.orientation ...
edit retag close merge delete

That's a very good point. XD Thanks, I'll do that. :)
I'm posting it there as well, however half the question is about the correct usage of the quaternions in PoseStamped ROS messages with rviz, so it's actually still valid here too. :)

Sort by » oldest newest most voted I know this does not answer your question, but there is a nodelet in pcl_ros that can segment planes: http://www.ros.org/wiki/pcl_ros/Tutorials/SACSegmentationFromNormals%20planar%20segmentation

more