MAP网格化算法思路

第一步：初始化一个种子三角面。（随机选点，基于该点进行临近搜索到第二点；在基于该线段中点临近搜索到第三点）

 

                                          图1

 

第二步：在种子三角面的基础上，进行面片的扩充，利用边的中点进行临近搜索，碰到合适的点，就会跟这条边构成一个新的三角面，同时构造出两条新边。依次类推… 直到队列中不再有满足条件外边提供中点检索为止。

 

第三步：寻找新的种子三角面，进行第二步；直到再也无法找到合适的种子三角面，退出循环。

第四步：输出mesh，包含生成三角面，以及原始点云。

 

                                                  图2

 

主要算法实现：

for (size_t i = 0; i < num; i+=10)
    {
        if (point_vect[i].size()>0)
        {
            continue;
        }

        PointType searchPoint = cloud_ptr->points[i];
        initializeTriangle(searchPoint);    //第一步  初始化一个种子三角面

        while (true)
        {
            if (edge_queue.empty())
            {
                //std::cout << "MPA::initializeTriangle()  failed ..." << std::endl;
                break;
            }

            PCTTM_Edge search_edge = edge_queue.front();
            bool bad_edge = false;
            for (size_t i = 0; i < point_vect[search_edge.p_index_first].size(); i++)
            {
                if (point_vect[search_edge.p_index_first][i].p_index == search_edge.p_index_end
                    && point_vect[search_edge.p_index_first][i].ptp_relation == 2)
                {
                    bad_edge = true;
                }
            }
            if (bad_edge)
            {
                edge_queue.pop();
                continue;
            }
            searchPointMPA(search_edge, 5);     //第二步  搜索点 扩展边
            edge_queue.pop();
        }
    }

 

实现结果：

17万个点，构建了32.7万个面，目前用时26.774秒。