0
$\begingroup$

Rosanswers logo

[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.y =  normal_x * std::sin(0.5 * std::asin(normal_z));
        normal.pose.orientation.z = 0;
        plane_normal_posestamped_pub_.publish(normal);

    }  // if (points.size() >= 3)
}  // Converter::publishLSPlane

Originally posted by Yo on ROS Answers with karma: 183 on 2012-01-11

Post score: 0


Original comments

Comment by Yo on 2012-01-12:
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. :)

Comment by Yo on 2012-01-12:
That's a very good point. XD Thanks, I'll do that. :)

Comment by karthik on 2012-01-12:
I think its better to post this question in http://www.pcl-users.org/

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by Stephan with karma: 1924 on 2012-05-13

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.