Ask Your Question
0

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

asked 2012-01-11 08:58:32 -0500

Yo gravatar image

updated 2012-01-12 12:24:50 -0500

[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 ...
(more)
edit retag flag offensive close merge delete

Comments

I think its better to post this question in http://www.pcl-users.org/
karthik gravatar imagekarthik ( 2012-01-12 07:00:59 -0500 )edit
That's a very good point. XD Thanks, I'll do that. :)
Yo gravatar imageYo ( 2012-01-12 11:50:50 -0500 )edit
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. :)
Yo gravatar imageYo ( 2012-01-12 12:14:01 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2012-05-13 21:57:52 -0500

Stephan gravatar image

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

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-01-11 08:58:32 -0500

Seen: 1,341 times

Last updated: Jan 12 '12