如何使用PCL将XYZRGB点云转换为彩色mesh模型

如何使用PCL将XYZRGB点云转换为彩色mesh模型

最近完成了一个使用RGBD传感器，构建物体模型的小demo。其中有点难的最后一步是如何将获得的物体点云变成彩色mesh模型。效果图如下（从点云变成彩色mesh）：


其实整体的步骤可以总结如下：
[1]计算点云法向,并将法向量指向内部
[2]将点云法向信息叠加在原点云上，生成pcl::PointXYZRGBNormal格式的点云
[3]使用泊松重建（poisson reconstruction）建立无颜色mesh。
[4]使用kdtree将原点云的信息映射在无颜色mesh上，并生成彩色mesh。

下面具体介绍一下各步骤：
[1]计算点云法向
使用pcl::NormalEstimationOMP设定法向估算对象。这里使用的算法实质上是主成分分析（PCA）。先设定每个点周围选取的临近点数和搜索半径，并用临近点建立一个协方差矩阵C。

这里K指的是离点P的最近的K个点，是最近邻的中心。后面的式子就是特征值和特征向量了。而C的最小特征值对应的特征向量就是该点的法向量。
但是泊松重建需要的是指向物体内部的法向量，所以我们还要将向量反转过来。

[2]将点云法向信息叠加在原点云上，生成pcl::PointXYZRGBNormal格式的点云
这一步比较简单，使用的是pcl::concatenateFields,可以将两种不同格式的点云组合起来。

[3]使用泊松重建（poisson reconstruction）建立无颜色mesh。
泊松重建的原理比较复杂，我也没有完全弄清楚。先留下这个坑，以后清楚了再填。大致的算法如下：
1、为点云设定八叉树搜索索引，使得每个采样点都落在深度为D的叶节点。
2、设定函数空间。
3、创建向量场。这一步我理解就是用到了之前算出的法向量。
4、求解泊松方程。
5、提取等值面，从而得到重建表面。
pcl中对应的是pcl::Poisson可以设定泊松处理对象。但是泊松重建后生成的mesh是没有RGB信息的。目前，PCL官方也说泊松重建不带有颜色信息，需要我们自己添加。

[4]使用kdtree将原点云的信息映射在无颜色mesh上，并生成彩色mesh。
这一步在网上基本找不到信息，所以我研究了一下。最终还是在一个user mailing的回答里得到了启发。我们可以使用kd tree在原RGB点云上建立一个搜索索引，然后对mesh上的每一个点都搜索对应原RGB点云上的临近点。然后求得这些对应临近点的RGB通道均值，作为mesh上那个点的颜色信息就好了。
这里值得注意的是两个转换函数pcl::fromPCLPointCloud2和pcl::toPCLPointCloud2，其作用分别是将mesh的信息转为点云和将点云信息转进mesh。

最后，就放一下源码啦。

void poisson_reconstruction(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr object_cloud)
{
      PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>());
      pcl::copyPointCloud(*object_cloud, *cloud);
      PointCloud<PointXYZRGB>::Ptr filtered(new PointCloud<PointXYZRGB>());
      PassThrough<PointXYZRGB> filter;
      filter.setInputCloud(cloud);
      filter.filter(*filtered);
      cout << "passthrough filter complete" << endl;

      cout << "begin normal estimation" << endl;
      NormalEstimationOMP<PointXYZRGB, Normal> ne;//计算点云法向
      ne.setNumberOfThreads(8);//设定临近点
      ne.setInputCloud(filtered);
      ne.setRadiusSearch(0.01);//设定搜索半径
      Eigen::Vector4f centroid;
      compute3DCentroid(*filtered, centroid);//计算点云中心
      ne.setViewPoint(centroid[0], centroid[1], centroid[2]);//将向量计算原点置于点云中心

      PointCloud<Normal>::Ptr cloud_normals (new PointCloud<Normal>());
      ne.compute(*cloud_normals);
      cout << "normal estimation complete" << endl;
      cout << "reverse normals' direction" << endl;

//将法向量反向
      for(size_t i = 0; i < cloud_normals->size(); ++i)
      {
        cloud_normals->points[i].normal_x *= -1;
        cloud_normals->points[i].normal_y *= -1;
        cloud_normals->points[i].normal_z *= -1;
      }

//融合RGB点云和法向
      cout << "combine points and normals" << endl;
      PointCloud<PointXYZRGBNormal>::Ptr cloud_smoothed_normals(new PointCloud<PointXYZRGBNormal>());
      concatenateFields(*filtered, *cloud_normals, *cloud_smoothed_normals);

//泊松重建
      cout << "begin poisson reconstruction" << endl;
      Poisson<PointXYZRGBNormal> poisson;
      //poisson.setDegree(2);
      poisson.setDepth(8);
      poisson.setSolverDivide (6);
      poisson.setIsoDivide (6);

      poisson.setConfidence(false); 
      poisson.setManifold(false); 
      poisson.setOutputPolygons(false); 

      poisson.setInputCloud(cloud_smoothed_normals);
      PolygonMesh mesh;
      poisson.reconstruct(mesh);

      cout << "finish poisson reconstruction" << endl;

//给mesh染色
      PointCloud<PointXYZRGB> cloud_color_mesh; 
      pcl::fromPCLPointCloud2(mesh.cloud, cloud_color_mesh); 

      pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
      kdtree.setInputCloud (cloud);
      // K nearest neighbor search
      int K = 5;
      std::vector<int> pointIdxNKNSearch(K);
      std::vector<float> pointNKNSquaredDistance(K);
      for(int i=0;i<cloud_color_mesh.points.size();++i)
      {
         uint8_t r = 0;
         uint8_t g = 0;
         uint8_t b = 0;
         float dist = 0.0; 
         int red = 0;
         int green = 0;
         int blue = 0;
         uint32_t rgb;

         if ( kdtree.nearestKSearch (cloud_color_mesh.points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
         {
            for (int j = 0; j < pointIdxNKNSearch.size (); ++j) 
            { 

               r = cloud->points[ pointIdxNKNSearch[j] ].r;
               g = cloud->points[ pointIdxNKNSearch[j] ].g;
               b = cloud->points[ pointIdxNKNSearch[j] ].b;

               red += int(r);
               green += int(g);
               blue += int(b);
               dist += 1.0/pointNKNSquaredDistance[j]; 

               std::cout<<"red: "<<int(r)<<std::endl; 
               std::cout<<"green: "<<int(g)<<std::endl; 
               std::cout<<"blue: "<<int(b)<<std::endl; 
               cout<<"dis:"<<dist<<endl;
            }  
         }

         cloud_color_mesh.points[i].r = int(red/pointIdxNKNSearch.size ()+0.5); 
         cloud_color_mesh.points[i].g = int(green/pointIdxNKNSearch.size ()+0.5); 
         cloud_color_mesh.points[i].b = int(blue/pointIdxNKNSearch.size ()+0.5);


      }
      toPCLPointCloud2(cloud_color_mesh, mesh.cloud);
      io::savePLYFile("/home/xxx/3d_model_building/src/make_model/result/object_mesh.ply", mesh);
}

如有问题，还请大家指出。

参考资料：
[1]http://blog.csdn.net/qq_25491201/article/details/51100073
[2]http://blog.csdn.net/jennychenhit/article/details/52126156?locationNum=8
[3]https://wenku.baidu.com/view/23c5637a31b765ce050814d2.html