SLAMesh论文及代码阅读与思考<三>Real-time LiDAR Simultaneous Localization and Meshing

前言

继续对SLAMesh的代码结构和细节进行梳理。

process()函数

启动程序后进入process()函数，在该函数中首先创建了两个重要的map对象

Map map_glb(Tguess)//主要用来存储全局的地图信息
Map map_now//主要用来存储当前点云对应的局部地图信息
map_now使用vector的数据结构，方便进行并行化，map_glb使用unordered_map来存储，方便进行快速查找和更新
下面看下分别对应的构造函数: step变量表示当前的帧的索引数，获取原始点云->变换到全局坐标系下->将新的点云分配到栅格中->初始化全局地图对应的栅格集合。我们可以看到其实在map_glb初始化的时候，就已经获取了第一帧点云了。在后面processNewScan的时候已经是第二帧点云了。

Map::Map(Transf & Tguess): step(0){
    //constructor of the first map glb
    ROS_DEBUG("Map");
    name = "MAP";
    //TicToc t_get_pcl, t_turn_point, t_gp;
    getPointCloud(points_turned, pcl_raw, timestamp, param.voxel_size);
    //std::cout<<"t_get_pcl:"<<t_get_pcl.toc() << "ms"<< std::endl;
    points_turned.transformPoints(Tguess);
    dividePointsIntoCellInitMap(points_turned);
    cells_glb.reserve(5000);
}

map_now的初始化就只包括了栅格集合的构建

Map::Map(){
    //constructor of the map now
    ROS_DEBUG("Map");
    name = "CURRENT_SCAN";
    cells_now.resize(pow(2 * (param.range_max / param.grid) + 2, 3) / 5);
}

然后开始进入循环，进入map_now..processNewScan(Tguess, g_data.step, map_glb)
该函数继续获取当前新的点云，变换到全局坐标系下，然后进入dividePointsIntoCell(points_turned, map_glb, true);
该函数的基本流程是：

1. 在世界坐标系下对当前新的点云，找到其bounding box的边界坐标
2. 计算bounding box包含的栅格数量，num_grid，然后和预设的bucket的规模进行对比，如果超过了预设的规模，那么就逐步增大bucket的规模直到和num_grid完全一致。
3. 快速的针对上一帧的痕迹来进行clear
4. 将当前点云添加到对应的bucket中，索引直接根据x,y,z来计算
5. 然后统计哪些bucket中有足够数量的点，以及哪些bucket中非空
6. 只对有足够数量点的buctet进行cell的构建与surface重建，存储在cell_now中，cell_now主要存储当前点云对应的cell的信息
7. 重点在tem_cell的构造，即

std::pair<double, Cell> tmp_cell (posi_tmp, Cell(points_raw_cell, g_data.step,
                                                        posi_tmp, region_tmp,
                                                        conduct_gp, map_glb_not_surface));
        tmp_cell.second.time_stamp = g_data.step;
        cells_now[index_bucket_enough_point[i_bucket_not_empty]] = tmp_cell;

其中的reconstructSurfaces()函数和gaussianProcess()函数。

这两个函数的详细过程后面再看。