用PCL进行点云的表面重建,用贪婪投影三角法进行网格化

从完全不知到弄出结果用了好几天的时间,虽然结果还不是很理想,但也值得纪念一下,总结一下过程中出现的问题。

按照上一篇博客中的算法代码(如下)就可以,问题处在pcd文件上,pcd文件有固定的书写格式,可以按程序生成,也可以自己写,只要格式没问题即可。

接下来就是储存位置问题,pcd文件要和可执行文件即.exe文件放在同一文件夹下。运行程序,即可看到输出结果。

#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include

int main (int argc, char** argv)
{
  // Load input file into a PointCloud with an appropriate type
  pcl::PointCloud::Ptr cloud (new pcl::PointCloud);


 if(pcl::io::loadPCDFile("mypointcloud.pcd",*cloud)==-1)//打开点云文件
 {
      PCL_ERROR("Couldn't read file mypointcloud.pcd\n");  //文件名要写对
      return -1;
 }

  //sensor_msgs::PointCloud2 cloud_blob;
  //pcl::io::loadPCDFile ("bun0.pcd", cloud_blob);
  //pcl::fromROSMsg (cloud_blob, *cloud);
  //* the data should be available in cloud
 
  // Normal estimation(法向量估计)
  pcl::NormalEstimation n;
  pcl::PointCloud::Ptr normals (new pcl::PointCloud);
  pcl::search::KdTree::Ptr tree (new pcl::search::KdTree);
  tree->setInputCloud (cloud);
  n.setInputCloud (cloud);
  n.setSearchMethod (tree);
  n.setKSearch (20);
  n.compute (*normals);  //法线
  //* normals should not contain the point normals + surface curvatures(不能同时包含点的法向量和表面的曲率)


  // Concatenate the XYZ and normal fields (将点云和法线放在一起)
  pcl::PointCloud::Ptr cloud_with_normals (new pcl::PointCloud);
  pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);


  // Create search tree*
  pcl::search::KdTree::Ptr tree2 (new pcl::search::KdTree);
  tree2->setInputCloud (cloud_with_normals);

  // Initialize objects (初始化对象)
  pcl::GreedyProjectionTriangulation gp3;
  pcl::PolygonMesh triangles;  //创建多边形网格,用于存储结果

  //设置参数
  gp3.setSearchRadius (1.5);  // 设置连接点之间的最大距离(最大边长)用于确定k近邻的球半径(默认为0)
  gp3.setMu (2.5);  // 设置最近邻距离的乘子,已得到每个点的最终搜索半径(默认为0)
  gp3.setMaximumNearestNeighbors (100);  //设置搜索的最近邻点的最大数量
  gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees 最大平面角
  gp3.setMinimumAngle(M_PI/18); // 10 degrees 每个三角的最大角度
  gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
  gp3.setNormalConsistency(false);  //若法向量一致,设为true

  // 设置搜索方法和输入点云
  gp3.setInputCloud (cloud_with_normals);
  gp3.setSearchMethod (tree2);

  //执行重构,结果保存在triangles中
  gp3.reconstruct (triangles);

  //保存网格图
  pcl::io::saveVTKFile("mymesh.vtk",triangles);   //这句可以没有

  // Additional vertex information
  std::vector parts = gp3.getPartIDs();
  std::vector states = gp3.getPointStates();
  fstream fs ;
    fs.open("partsID.txt" , ios::out);
    if (!fs)
    {
        return -2;
    }
    fs<<"点云数量为:"<     for (int i = 0 ; i < parts.size() ; i++)
    {
        if (parts[i] != 0)
        {
            fs<         }       
    }

  //显示结果图
  boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer("3D Viewer"));
  viewer->setBackgroundColor(0,0,0);  //设置背景
  viewer->addPolygonMesh(triangles,"my");  //设置显示的网格
  viewer->addCoordinateSystem(1.0);  //设置坐标系
  viewer->initCameraParameters();
  while(!viewer->wasStopped())
  {
      viewer->spinOnce(100);
 boost::this_thread::sleep (boost::posix_time::microseconds(100000));
  }


  // Finish
  return 0;
}

你可能感兴趣的:(重建算法)