ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

PCL SACSegmentation - setAxis and setModelType has no effect in Output

asked 2013-04-29 20:41:56 -0500

KarthikMurugan gravatar image

updated 2014-01-28 17:16:23 -0500

ngrennan gravatar image

I want to estimate the ground plane present in the pointcloud and remove it. I used the plane model segmentation tutorial for that.

Instead of removing the ground plane it removes the vertical planes(wall) in the pointcloud.

I changed the axis perpendicular to which the plane is estimated but get the same result..

This is the code :-

  Eigen::Vector3f axis = Eigen::Vector3f(0.0,1.0,0.0);

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZI> seg;
   seg.setAxis(axis);

  seg.setOptimizeCoefficients (true);

  seg.setModelType (pcl:: SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.5);

  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);

When I input a stream of pointclouds through ROS, I get Model coefficients as :-

Model coefficients: 0.980676 0.00643671 0.195532 0.127706
Model coefficients: 0.0974007 -0.00814183 0.995212 2.64382
Model coefficients: 0.0235566 -0.997725 -0.0631705 4.7102
Model coefficients: 0.0404465 -0.999113 -0.0117141 4.65797

ie sometimes it removes ground plane and at some other instant it removes walls... the coordinate system in the pointcloud is

x axis -> forward
y axis -> right
z axis -> up

Now since ground plane is along x-z plane perpendicular to y axis I set the Axis as (0.0,1.0,0.0) so that according to PCL api ground_plane perpendicular to y axis be removed... but as i said only at some instants it is removed and at other instants vetical planes (wall) gets removed...

I tried all possibilities of (1,0,0) ,(0,1,0),(0,0,1) but it has NO EFFECT in the output result...The same result I mentioned above is obtained...

Also changed ModelType to SACMODEL_PERPENDICULAR_PLANE , SACMODEL_PARALLEL_PLANE ,still no desired result

Please pour in your suggestions where I am going wrong..

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2013-04-30 02:46:37 -0500

I think you need to also set the eps_angle parameter.

If you check the source code for sac_model_parallel_plane.hpp, you'll see that the axis is only used in the isModelValid() function. And, even then, only when eps_angle_ > 0. Looking in sac_model_parallel_plane.h, we see that eps_angle_ is initialized to 0. So, unless you explicitly set this parameter, the model will effectively ignore the parallel-to-axis constraint.

Try adding this:

seg.setEpsAngle(  30.0f * (M_PI/180.0f) );
edit flag offensive delete link more

Comments

Thank you Jeremy.. So is the angle parameter signify angle tolerance of plane with the axis or its normal with the axis??? (as it is in this : http://docs.pointclouds.org/trunk/classpcl_1_1_s_a_c_segmentation.html )

KarthikMurugan gravatar image KarthikMurugan  ( 2013-04-30 18:54:13 -0500 )edit

Read the PCL documentation for setEpsAngle() at the link you provided. It is: "maximum allowed difference between the model NORMAL and the given axis in radians". So, the "axis" parameter is the expected plane normal vector, and "epsAngle" is the allowable variation for a plane to count as "valid"

Jeremy Zoss gravatar image Jeremy Zoss  ( 2013-04-30 19:27:20 -0500 )edit

Thank u Jeremy.... tat worked!!! now only the ground plane gets removed...

but Still at some instants nothing gets removed... do you know wat could be the reason behind it... I tried values for epsAngle from 20 to 45 ....any rectification to be made ??

KarthikMurugan gravatar image KarthikMurugan  ( 2013-05-01 07:04:59 -0500 )edit

Also what is the difference between SACMODEL_PERPENDICULAR_PLANE and SACMODEL_PARALLEL_PLANE... I get the same result for both!!!

KarthikMurugan gravatar image KarthikMurugan  ( 2013-05-01 07:07:43 -0500 )edit

Your questions are fairly specific to PCL, and are not really ROS-related. You should try addressing these questions to the PCL user's list: pcl-users@pointclouds.org.

Jeremy Zoss gravatar image Jeremy Zoss  ( 2013-05-01 07:11:34 -0500 )edit

hi i have the same problem. In my pointcloud, the wall is a dominant plane, everytime i tried to extract the ground (set axis method) it still gives me the wall. How did you solve your problem

dmngu9 gravatar image dmngu9  ( 2015-04-22 19:06:58 -0500 )edit

I also have the same problem. It should return a plane that is within the 'epsAngle' of the specified 'axis' even if one doesn't fit well, right? It seems like what is happening is that there are 0 inliers in the model that is found (which should be an error condition?).

daviddoria gravatar image daviddoria  ( 2015-12-02 16:40:11 -0500 )edit
0

answered 2017-05-23 13:47:33 -0500

biglotusturtle gravatar image

I was having this problem as well so if anyone is interested here is how I solved it.

I was also looking for a ground plane in my point cloud and because of my cameras coordinates frame I wanted the X-Z plane.

pcl::SACSegmentation<pcl::PointXYZRGB> seg;

//Set up parameters for our segmentation/ extraction scheme
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE ); //only want points perpendicular to a given axis
seg.setMaxIterations(500); // this is key (default is 50 and that sucks)
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.05); // keep points within 0.05 m of the plane

//because we want a specific plane (X-Z Plane) (In camera coordinates the ground plane is perpendicular to the y axis)
Eigen::Vector3f axis = Eigen::Vector3f(0.0,1.0,0.0); //y axis
seg.setAxis(axis);
seg.setEpsAngle(  30.0f * (PI/180.0f) ); // plane can be within 30 degrees of X-Z plane

The key for me ended up being the maxIterations parameter. The default I believe is 50 iterations and it would not find the correct plane with so few. I found that at a minimum I needed setMaxIterations(500) before it found the correct plane.

Hope this helps.

edit flag offensive delete link more

Question Tools

Stats

Asked: 2013-04-29 20:41:56 -0500

Seen: 10,274 times

Last updated: May 23 '17