Detect Floor Plane with PCL

[Eduard Kosel]

Hi!

I'm trying to detect the floor Plane with my Kinect via PCL. My ASUS Xtion Pro Live is mounted stationary and is not moved during the process. The Floor is the dominating plane in the scene.

void ToolKit::getGroundPlane(ntk::RGBDImage& image){

   

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

rgbdImageToPointCloud(*cloud, image, true /* keep dense */);

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);

pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

pcl::SACSegmentation<pcl::PointXYZ> seg;

seg.setOptimizeCoefficients(true);

seg.setModelType(pcl::SACMODEL_PLANE);

seg.setMethodType(pcl::SAC_RANSAC);

seg.setDistanceThreshold(0.05);

seg.setInputCloud(cloud->makeShared());

seg.segment (*inliers, *coefficients);

if (inliers->indices.size() == 0){

PCL_ERROR ("Could not estimate a planar model for the given dataset.");

}

setGroundPlane(ntk::Plane(coefficients->values[0],coefficients->values[1],coefficients->values[2],coefficients->values[3]));

std::cout << m_ground_plane.a << "," << m_ground_plane.b << "," << m_ground_plane.c << "," << m_ground_plane.d << "\n";

setGroundIndices(inliers->indices);

}

However the plane coefficients (a,b,c,d)  given by the coefficients seem to be incorrect since i cannot correctly remove them from my depth image. I think that is calculating correctly since I can remove the inlier pixels (inliers->indices) from the depth image and only the ground is gone. I guess that the problem seems to be some kind of coordinate transformation of the planar coefficients. Did anybody solve this problem yet or can suggest a way to solve this?

Cheers

Eduard

[Nicolas Burrus]

Hi Eduard,

You might want to have a look at the TableObjectDetector class
(ntk/detection/table_object_detector.h), that exacts a plane, and the
clusters of objects on top of it.

What exactly do you want to remove if it is not the plane inliers?

Cheers,
Nicolas

[Eduard Kosel]

Hi Nicolas!

Always enjoy talking to you btw :) - Yeah, the task I'm trying to accomplish is a bit complicated to explain but I'll give it a try.

I'm trying to obtain the depth data for a soccer-ball which is always lying on the same spot for a certain amount of time (i.e. for a penalty kick in a soccer match) till it's kicked :-D.

I tried to do the following steps:

1. Get the real world coordinates of the penalty-kick point by clicking and transforming to real world coordinates (this works)
2. Get the plane coordinates from the floor and with it get the normal vector of the floor ( a,b,c values of pcl plane model coefficient, d only needed for the next step)
3. Save values from the previous step in an ntk::Plane and apply distanceToPoint(cv::Point3d) to every Point of the depth image to remove the floor (this will cancel out costly reiteration for PCL plane segmentation as it only uses some light linear algebra)
4. Next step is to define a circular or squared area around the penalty-kick point and remove everything which is lying outward of it
5. Get an image with only the ball in it (want to run the generation of color-range on it and hence need an almost perfect mask which I can get from said depth image)

I'll look into the said file you suggested and report back if it worked. If you have any suggestions for improvement I'm happy for every hint I could get from you :)

Cheers

Eduard

[Nicolas Burrus]

Hi,

Sounds like a great app! Maybe your issue is just related to the
coordinate system change between the depth image (x right, y down, z
forward) and the 3D system on the PCL side (x right, y up, z
backward)?

Good luck,
Nicolas

[Eduard Kosel]

Actually I guess that this is the problem. Do you know an easy way to solve this problem? Nice to hear that you like the project. I'll send you the source code as soon as we got something running :)

[Eduard Kosel]

If I understood you correctly the plane should be rotated by 180° around the x-axis to get the plane inside the depth coordinate system? How do I rotate a plane in ax + by + cz + d = 0 form...? Rotating every Point seems to be a bit overkill :D

Cheers Eduard

[Nicolas Burrus]

It's probably easier to do it in the (normal, point_on_plane) form.
Just apply the rotation to the normal and the plane point, and then
create the new Plane from it.

There might be a more direct way though...

[Eduard Kosel]

Hi Nicolas!

I had some time to work at the problem. I figured out that PCL is invariant to the camera tilt (i.e. when I draw the extracted Plane into a MeshViewer Widget it aligns with the floor perfectly). However when I rotate it into the OpenNI Coordinate System it is not simply a 180° rotation around the x-Axis. The camera tilt seems to be important regarding this. Do you know how I can eliminate the camera tilt?

Cheers Eduard