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

edit

[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.

Would someone please help check me on this?

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!

void Converter::publishLSPlane(pcl::PointCloud<pcl::PointXYZ> points, std_msgs::Header header) {

    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.header = header;
        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 ...