首页 > 代码库 > PCL源码剖析之MarchingCubes算法

PCL源码剖析之MarchingCubes算法

原文:http://blog.csdn.net/lming_08/article/details/19432877

MarchingCubes算法简介

MarchingCubes(移动立方体)算法是目前三围数据场等值面生成中最常用的方法。它实际上是一个分而治之的方法,把等值面的抽取分布于每个体素中进行。对于每个被处理的体素,以三角面片逼近其内部的等值面片。每个体素是一个小立方体,构造三角面片的处理过程对每个体素都“扫描”一遍,就好像一个处理器在这些体素上移动一样,由此得名移动立方体算法。

MC算法主要有三步:1.将点云数据转换为体素网格数据;2.使用线性插值对每个体素抽取等值面;3.对等值面进行网格三角化

 

PCL源码剖析之MarchingCubesHoppe

PCL中使用MarchingCubesHoppe类进行三维重建执行的函数体为performReconstruction(),其代码如下:

 

[cpp] view plaincopyprint?在CODE上查看代码片派生到我的代码片
 
  1. template <typename PointNT> void  
  2. pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output)  
  3. {  
  4.   if (!(iso_level_ >= 0 && iso_level_ < 1))  
  5.   {  
  6.     PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);  
  7.     output.cloud.width = output.cloud.height = 0;  
  8.     output.cloud.data.clear ();  
  9.     output.polygons.clear ();  
  10.     return;  
  11.   }  
  12.   
  13.   // Create grid  
  14.   grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);  
  15.   
  16.   // Populate tree  
  17.   tree_->setInputCloud (input_);  
  18.   
  19.   getBoundingBox ();  
  20.   
  21.   // Transform the point cloud into a voxel grid  
  22.   // This needs to be implemented in a child class  
  23.   voxelizeData ();  
  24.   
  25.   
  26.   
  27.   // Run the actual marching cubes algorithm, store it into a point cloud,  
  28.   // and copy the point cloud + connectivity into output  
  29.   pcl::PointCloud<PointNT> cloud;  
  30.   
  31.   for (int x = 1; x < res_x_-1; ++x)  
  32.     for (int y = 1; y < res_y_-1; ++y)  
  33.       for (int z = 1; z < res_z_-1; ++z)  
  34.       {  
  35.         Eigen::Vector3i index_3d (x, y, z);  
  36.         std::vector<float> leaf_node;  
  37.         getNeighborList1D (leaf_node, index_3d);  
  38.         createSurface (leaf_node, index_3d, cloud);  
  39.       }  
  40.   pcl::toPCLPointCloud2 (cloud, output.cloud);  
  41.   
  42.   output.polygons.resize (cloud.size () / 3);  
  43.   for (size_t i = 0; i < output.polygons.size (); ++i)  
  44.   {  
  45.     pcl::Vertices v;  
  46.     v.vertices.resize (3);  
  47.     for (int j = 0; j < 3; ++j)  
  48.       v.vertices[j] = static_cast<int> (i) * 3 + j;  
  49.     output.polygons[i] = v;  
  50.   }  
  51. }  

可以看出PCL将会产生res_x_ * res_y_ * res_z_个网格,即为Resolution分辨率。voxelizeData ();即将点云数据转换为体素网格数据,其实现如下:

 

 

[cpp] view plaincopyprint?在CODE上查看代码片派生到我的代码片
 
  1. template <typename PointNT> void  
  2. pcl::MarchingCubesHoppe<PointNT>::voxelizeData ()  
  3. {  
  4.   for (int x = 0; x < res_x_; ++x)  
  5.     for (int y = 0; y < res_y_; ++y)  
  6.       for (int z = 0; z < res_z_; ++z)  
  7.       {  
  8.         std::vector<int> nn_indices;  
  9.         std::vector<float> nn_sqr_dists;  
  10.   
  11.         Eigen::Vector3f point;  
  12.         point[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (x) / float (res_x_);  
  13.         point[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (y) / float (res_y_);  
  14.         point[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (z) / float (res_z_);  
  15.   
  16.         PointNT p;  
  17.         p.getVector3fMap () = point;  
  18.   
  19.         tree_->nearestKSearch (p, 1, nn_indices, nn_sqr_dists);  
  20.   
  21.         grid_[x * res_y_*res_z_ + y * res_z_ + z] = input_->points[nn_indices[0]].getNormalVector3fMap ().dot (  
  22.             point - input_->points[nn_indices[0]].getVector3fMap ());  
  23.       }  
  24. }  

该函数对每个体素网格数据进行赋值,其值为一符号距离函数值,其定义为:f(Pi) = (Pi - Oi) * N(Oi), 这里Pi为给定的点,Oi为Pi周围K近邻点集(输入点云的子集)的中心, N(Oi)为点Oi的法向量,中间的*为数量积;求出的值其实是点Oi到过点Pi的有向切平面的距离,图示如下:

 

点q处的法向量是单位法向量,所以点q到切平面的距离是dot(N(p), vec(p, q))

从代码中可以看出,这里的K = 1,即求出最近邻点。

 

下面的代码描述了对每个体素网格的处理过程,其主要过程是计算出每个体素网格与等值面的交点,然后按一定顺序将交点连接,从而形成三角面片。

 

[cpp] view plaincopyprint?在CODE上查看代码片派生到我的代码片
 
  1. for (int x = 1; x < res_x_-1; ++x)  
  2.   for (int y = 1; y < res_y_-1; ++y)  
  3.     for (int z = 1; z < res_z_-1; ++z)  
  4.     {  
  5.       Eigen::Vector3i index_3d (x, y, z);  
  6.       std::vector<float> leaf_node;  
  7.       getNeighborList1D (leaf_node, index_3d);  
  8.       createSurface (leaf_node, index_3d, cloud);  
  9.     }  

getNeighorList1D(leaf_node, index_3d);即是求出当前体素网格的8个顶点对应符号距离函数值,即数组grid_中对应的值。实现代码如下:

[cpp] view plaincopyprint?在CODE上查看代码片派生到我的代码片
 
  1. template <typename PointNT> void  
  2. pcl::MarchingCubes<PointNT>::getNeighborList1D (std::vector<float> &leaf,  
  3.                                                 Eigen::Vector3i &index3d)  
  4. {  
  5.   leaf = std::vector<float> (8, 0.0f);  
  6.   
  7.   leaf[0] = getGridValue (index3d);  
  8.   leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));  
  9.   leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1));  
  10.   leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1));  
  11.   leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0));  
  12.   leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));  
  13.   leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));  
  14.   leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));  
  15. }  

 

createSurface (leaf_node, index_3d, cloud);即是求出每个体素网格与等值面的交点,然后按一定顺序将交点连接,从而形成三角面片。下面分片段剖析createSurface ()函数代码。

 

[cpp] view plaincopyprint?在CODE上查看代码片派生到我的代码片
 
  1. int cubeindex = 0;  
  2. Eigen::Vector3f vertex_list[12];  
  3. if (leaf_node[0] < iso_level_) cubeindex |= 1;  
  4. if (leaf_node[1] < iso_level_) cubeindex |= 2;  
  5. if (leaf_node[2] < iso_level_) cubeindex |= 4;  
  6. if (leaf_node[3] < iso_level_) cubeindex |= 8;  
  7. if (leaf_node[4] < iso_level_) cubeindex |= 16;  
  8. if (leaf_node[5] < iso_level_) cubeindex |= 32;  
  9. if (leaf_node[6] < iso_level_) cubeindex |= 64;  
  10. if (leaf_node[7] < iso_level_) cubeindex |= 128;  

此段代码是将8个顶点的标量值与等值面相比较,如果标量值小于等值面值(即顶点在等值面下面),则将cubeindex相应的位置为1。这样就可以知道8个顶点中哪些在等值面下,哪些在等值面之上了。
立方体中顶点与棱边的编号如下所示:

 

例如,如果顶点3的值在等值面值之下并且所有其他顶点的值都在等值面之上,那么我们可以通过切割边2、3、11来创建一个三角面片。

算法使用一个边表将cubeindex映射为一个12bit的数值,每一位与一条边相关,如果边没有被等值面切割则设为0,切割则设为1。如果没有边被切割那么表返回0,这种情况发生在当cubeindex = 0(所有顶点在等值面之下)或0xff(所有顶点在等值面之上)。举个之前的例子,如果只有顶点3在等值面下面,cubeindex将会等于0000 1000 或8。边表edgeTable定义在marching_cubes.h文件中:

 

[cpp] view plaincopyprint?在CODE上查看代码片派生到我的代码片
 
  1. const unsigned int edgeTable[256] = {  
  2.   0x0  , 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,  
  3.   0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,  
  4.   0x190, 0x99 , 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,  
  5.   0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,  
  6.   0x230, 0x339, 0x33 , 0x13a, 0x636, 0x73f, 0x435, 0x53c,  
  7.   0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,  
  8.   0x3a0, 0x2a9, 0x1a3, 0xaa , 0x7a6, 0x6af, 0x5a5, 0x4ac,  
  9.   0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,  
  10.   0x460, 0x569, 0x663, 0x76a, 0x66 , 0x16f, 0x265, 0x36c,  
  11.   0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,  
  12.   0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff , 0x3f5, 0x2fc,  
  13.   0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,  
  14.   0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55 , 0x15c,  
  15.   0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,  
  16.   0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc ,  
  17.   0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,  
  18.   0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,  
  19.   0xcc , 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,  
  20.   0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,  
  21.   0x15c, 0x55 , 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,  
  22.   0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,  
  23.   0x2fc, 0x3f5, 0xff , 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,  
  24.   0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,  
  25.   0x36c, 0x265, 0x16f, 0x66 , 0x76a, 0x663, 0x569, 0x460,  
  26.   0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,  
  27.   0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa , 0x1a3, 0x2a9, 0x3a0,  
  28.   0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,  
  29.   0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33 , 0x339, 0x230,  
  30.   0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,  
  31.   0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99 , 0x190,  
  32.   0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,  
  33.   0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0  
  34. };  

 

edgeTable[8] = 0x80c = 1000 0000 1100。这就表示边2、3、11与等值面相交。
判断出边与等值面相交之后,就要确定具体的交点。这里PCL使用线性插值,线性插值具体见线性插值。代码如下:

 

[cpp] view plaincopyprint?在CODE上查看代码片派生到我的代码片
 
  1. // Find the vertices where the surface intersects the cube  
  2. if (edgeTable[cubeindex] & 1)  
  3.   interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);  
  4. if (edgeTable[cubeindex] & 2)  
  5.   interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]);  
  6. if (edgeTable[cubeindex] & 4)  
  7.   interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]);  
  8. if (edgeTable[cubeindex] & 8)  
  9.   interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]);  
  10. if (edgeTable[cubeindex] & 16)  
  11.   interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]);  
  12. if (edgeTable[cubeindex] & 32)  
  13.   interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]);  
  14. if (edgeTable[cubeindex] & 64)  
  15.   interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]);  
  16. if (edgeTable[cubeindex] & 128)  
  17.   interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]);  
  18. if (edgeTable[cubeindex] & 256)  
  19.   interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]);  
  20. if (edgeTable[cubeindex] & 512)  
  21.   interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]);  
  22. if (edgeTable[cubeindex] & 1024)  
  23.   interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]);  
  24. if (edgeTable[cubeindex] & 2048)  
  25.   interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]);  



 

 

文章参考于:http://www.doc88.com/p-5475997688638.html

                        http://paulbourke.net/geometry/polygonise/

                         http://books.google.com.hk/books?id=4k4kvDwP-lgC&printsec=frontcover&hl=zh-CN#v=onepage&q&f=false

PCL源码剖析之MarchingCubes算法