PCL采样一致性算法

admin

1. {  //Open 3D viewer and add point cloud-----  //
2.   boost::shared采用ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
3. viewer->setBackgroundColor (0, 0, 0);
4. viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
5. viewer->setPointCloudRenderingProperties (pcl::visualization::PCL采用VISUALIZER采用POINT采用SIZE, 3, "sample cloud");
6. //viewer->addCoordinateSystem (1.0, "global");
7. viewer->initCameraParameters ();  return (viewer);
8. }
9. /*对点云进行初始化，并对其中一个点云填充点云数据作为处理前的的原始点云，其中大部分点云数据是基于设定的圆球和平面模型计算而得到的坐标值作为局内点，有1/5的点云数据是被随机放置的组委局外点。*/
10. intmain(int argc, char** argv)
11. {  // 初始化点云对象
12. pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);  //存储源点云
13. pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);   //存储提取的局内点  // 填充点云数据
14. cloud->width    = 500;                 //填充点云数目
15.   cloud->height   = 1;                     //无序点云
16. cloud->is采用dense = false;
17. cloud->points.resize (cloud->width * cloud->height);  for (size采用t i = 0; i < cloud->points.size (); ++i)
18. {    if (pcl::console::find采用argument (argc, argv, "-s") >= 0 || pcl::console::find采用argument (argc, argv, "-sf") >= 0)
19.    {//根据命令行参数用x^2+y^2+Z^2=1设置一部分点云数据，此时点云组成1/4个球体作为内点
20.      cloud->points[i].x = 1024 * rand () / (RAND采用MAX + 1.0);
21.      cloud->points[i].y = 1024 * rand () / (RAND采用MAX + 1.0);     
22.   if (i % 5 == 0)
23.        cloud->points[i].z = 1024 * rand () / (RAND采用MAX + 1.0);   //此处对应的点为局外点
24.      else if(i % 2 == 0)
25.        cloud->points[i].z =  sqrt( 1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y));     
26.       else
27.        cloud->points[i].z =  - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y));
28.    }   
29.      else
30.    { //用x+y+z=1设置一部分点云数据，此时地拿云组成的菱形平面作为内点
31.      cloud->points[i].x = 1024 * rand () / (RAND采用MAX + 1.0);
32.      cloud->points[i].y = 1024 * rand () / (RAND采用MAX + 1.0);     
33.    if( i % 2 == 0)
34.     cloud->points[i].z = 1024 * rand () / (RAND采用MAX + 1.0);   //对应的局外点
35.   else
36.     cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
37.    }
38. } std::vector<int> inliers;  //存储局内点集合的点的索引的向量  
39.   //创建随机采样一致性对象
40. pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
41.    model采用s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));      //针对球模型的对象
42. pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
43.    model采用p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));     //针对平面模型的对象
44. if(pcl::console::find采用argument (argc, argv, "-f") >= 0)
45. {  //根据命令行参数，来随机估算对应平面模型，并存储估计的局内点
46.    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model采用p);
47.    ransac.setDistanceThreshold (.01);//与平面距离小于0.01 的点称为局内点考虑
48.    ransac.computeModel();                   //执行随机参数估计
49.    ransac.getInliers(inliers);                 //存储估计所得的局内点  }  else if (pcl::console::find采用argument (argc, argv, "-sf") >= 0 )
50. {
51.   //根据命令行参数  来随机估算对应的圆球模型，存储估计的内点
52.    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model采用s);
53.    ransac.setDistanceThreshold (.01);
54.    ransac.computeModel();
55.    ransac.getInliers(inliers);
56. }  // 复制估算模型的所有的局内点到final中
57. pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);  // 创建可视化对象并加入原始点云或者所有的局内点
58. boost::shared采用ptr<pcl::visualization::PCLVisualizer> viewer;  if (pcl::console::find采用argument (argc, argv, "-f") >= 0 || pcl::console::find采用argument (argc, argv, "-sf") >= 0)
59.    viewer = simpleVis(final);  else
60.    viewer = simpleVis(cloud);  while (!viewer->wasStopped ())
61. {
62.    viewer->spinOnce (100);
63.    boost::this采用thread::sleep (boost::posix采用time::microseconds (100000));
64. }  return 0;
65. }

复制代码