[ros-users] Bug in Point Cloud Library: MLESAC

[PinakiBanerjee]

Hi, I tried using MLESAC for SACMODEL_PLANE for segmentation of point cloud
dataset : table_scene_lms400.pcd from ROS. Unfortunately it fails to detect
the planes as can be seen in the image. This is the result obtained for 2000
iterations :

http://ros-users.122217.n3.nabble.com/file/n999095/MLESAC_2000.png 

Color convention : Green: Plane 1, Red: Plane 2., White: Remaining pixels.


Although the result of RANSAC for 50 iteration is accpetable as can be seen
below:

http://ros-users.122217.n3.nabble.com/file/n999095/RANSAC_50.png 


The code i used to prodece it is given below:

#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"

#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/model_types.h"
#include "pcl/segmentation/sac_segmentation.h"
#include "fstream"

using namespace std;

/* ---[ */
int
  main (int argc, char** argv)
{

//-----------------------Reading the data-----------------------------
sensor_msgs::PointCloud2 cloud_blob;
  pcl::PointCloud<pcl::PointXYZ> cloud;

  if (pcl::io::loadPCDFile ("table_scene_lms400.pcd", cloud_blob) == -1)
  {
    ROS_ERROR ("Couldn't read file test_pcd.pcd");
    return (-1);
  }
  ROS_INFO ("Loaded %d data points from test_pcd.pcd with the following
fields: %s", (int)(cloud_blob.width * cloud_blob.height), pcl::getFieldsList
(cloud_blob).c_str ());

 // Convert to the templated message type
  point_cloud::fromMsg (cloud_blob, cloud);


//-----------------------Appliying the segmentation
algorithm-------------------------

  pcl::ModelCoefficients coefficients;
  pcl::PointIndices inliers;
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_MLESAC);
  seg.setDistanceThreshold (0.01);
  seg.setMaxIterations (2000);
  seg.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ>
>(cloud));

time_t tstart, tend;

tstart = time(NULL);
  seg.segment (inliers, coefficients);
tend = time(NULL);
ROS_INFO("Segmentation 1 Time elapsed %f", difftime(tend,tstart));

  if (inliers.indices.size () == 0)
  {
    ROS_ERROR ("Could not estimate a planar model for the given dataset.");
    return (-1);
  }
ROS_INFO ("Done segmentation of initial cloud");
 ROS_INFO("No. of Inliers1 :  %d",inliers.indices.size ());

//--------------------------writing the
inliers------------------------------


ofstream SaveFile("inliers1_table_MLESAC_2000.txt");

for (size_t i = 0; i < inliers.indices.size (); ++i){
SaveFile << cloud.points[inliers.indices[i]].x << " " <<
cloud.points[inliers.indices[i]].y << " "
<<cloud.points[inliers.indices[i]].z << endl;
}

SaveFile.close();

ROS_INFO ("Results of segmentation of initial cloud written");

//---------------------------------------------------------------------------

pcl::PointCloud<pcl::PointXYZ> cloud1;

cloud1.width = cloud.width - inliers.indices.size();
cloud1.height = 1;
cloud1.points.resize (cloud1.width * cloud1.height);

int first,mid,last,val,key,index;
int a = 0;
for(index= 0 ; index<cloud.width; index++ ){

val=-1;
first=0;
last =inliers.indices.size()-1;
key=index;

   while (first <= last) {
       mid = (first + last) / 2;  // compute mid point.
       if (key > inliers.indices[mid]) 
           first = mid + 1;  // repeat search in top half.
       else if (key < inliers.indices[mid]) 
           last = mid - 1; // repeat search in bottom half.
       else{		
           val=0;     // found it. return position /////
	   break;
	}
   }

	if(val==-1){
	
	cloud1.points[a].x = 	cloud.points[index].x;
	cloud1.points[a].y = 	cloud.points[index].y;
	cloud1.points[a].z = 	cloud.points[index].z;
	a++;
	}
}
cout<< a << " =  " << cloud1.width;
ROS_INFO ("Cloud2 created successfully");

//--------------------------writing the modified
cloud------------------------------


ofstream SaveFile3("cloud2_table_MLESAC_2000.txt");

for (size_t i = 0; i < cloud1.width; ++i){
SaveFile3 << cloud.points[i].x << " " << cloud.points[i].y << " "
<<cloud.points[i].z << endl;
}

SaveFile3.close();

ROS_INFO ("cloud2 written");


//-----------------------Appliying the segmentation algorithm on the
modified cloud-------------------------




  seg.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ>
>(cloud1));
  pcl::PointIndices inliers1;
	
tstart = time(0);
  seg.segment (inliers1, coefficients);
tend = time(0);

ROS_INFO("Segmentation 2 Time elapsed %f", difftime(tend,tstart));

  if (inliers1.indices.size () == 0)
  {
    ROS_ERROR ("Could not estimate a planar model for the given dataset.");
    return (-1);
  }

 ROS_INFO("No. of Inliers2 :  %d",inliers1.indices.size ());
ROS_INFO ("Cloud2 segmented successfully");
//--------------------------writing the
inliers------------------------------//


ofstream SaveFile1("inliers2_table_MLESAC_2000.txt");

for (size_t i = 0; i < inliers1.indices.size (); ++i)
SaveFile1 << cloud1.points[inliers1.indices[i]].x << " " <<
cloud1.points[inliers1.indices[i]].y << " "
<<cloud1.points[inliers1.indices[i]].z << endl;

SaveFile1.close();

ROS_INFO ("Cloud2 written successfully");
//---------------------------------------------------------------------------

  return (0);
}
/* ]--- */


Is it a bug in the program? or am i missing some thing? Please help.
-- 
View this message in context: http://ros-users.122217.n3.nabble.com/Bug-in-Point-Cloud-Library-MLESAC-tp999095p999095.html
Sent from the ROS-Users mailing list archive at Nabble.com.

------------------------------------------------------------------------------
The Palm PDK Hot Apps Program offers developers who use the
Plug-In Development Kit to bring their C/C++ apps to Palm for a share 
of $1 Million in cash or HP Products. Visit us here for more details:
http://ad.doubleclick.net/clk;226879339;13503038;l?
http://clk.atdmt.com/CRS/go/247765532/direct/01/
_______________________________________________
ros-users mailing list
ros-users at lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/ros-users