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

wangjk-ustc / 2023-09-05 / 原文

前言

本节继续对update()函数进行理解。update是将当前点云对应的map_now对全局地图map_glb进行更新。

获取重叠的区域

OverlapCellsRelation overlap_ship = map_now.overlapCells(*this);

OverlapCellsRelation Map::overlapCells(Map & map_glb){
    // find overlapped cells between map_glb and map_now
    ROS_DEBUG("overlapCells");
    TicToc t_overlap_region;
    OverlapCellsRelation overlap_ship;
    //g_data.un_bounded_count = 0;

    for(int i = 0; i < index_bucket_enough_point.size(); i++){
        std::pair<double, Cell> & i_cell_now = cells_now[index_bucket_enough_point[i]];
        //don't consider revisit
        auto i_cell_glb = map_glb.cells_glb.find(i_cell_now.second.hash_position);
        if(i_cell_glb != map_glb.cells_glb.end() && (param.num_margin_old_cell < 0 ||
            std::abs(g_data.step - i_cell_glb->second.time_stamp) < param.num_margin_old_cell)){
            //if(i_cell_glb != map_glb.cells_glb.end() && (!param.margin_old_cell || g_data.step - i_cell_glb->second.time_stamp < 500)){
            Cell *ptr_cell_glb = &(i_cell_glb->second);
            Cell *ptr_cell_now = &(i_cell_now.second);
            overlap_ship.cells_glb.push_back(ptr_cell_glb);
            overlap_ship.cells_now.push_back(ptr_cell_now);
        }
        else{
            Cell* ptr_cell_new = &(i_cell_now.second);
            overlap_ship.cells_now_new.push_back(ptr_cell_new);
        }
    }
    g_data.time_find_overlap(0, g_data.step) += t_overlap_region.toc();
    return overlap_ship;
}

流程如下

对于当前栅格集合中每一个包含了足够多的点的栅格，根据其hash索引找到其在全局地图中对应的栅格
如果能找到，那么将两个来自全局和局部地图的相对应的栅格存储在overlap_ship的两个成员变量中
否则，就是当前的有效cell在全局地图中没有对应的cell，那么就将其存储在overlap_ship另外一个成员变量中

然后对于已经找到的重叠区域的栅格(局部地图中的)，如果其满足cell_glb.not_surface && !cell_now.not_surface为Ture，那么就将两者的点云合并，再进行重建
否则就直接进行顶点更新

for(int i_dir = 0; i_dir < 3; i_dir ++){
                    if(cell_glb.ary_cell_vertices[i_dir].num_point != 0 &&
                       cell_now.ary_cell_vertices[i_dir].num_point != 0 ) {
                        cell_glb.updateVertices(cell_now, Direction(i_dir));
                    }
                    else if(cell_glb.ary_cell_vertices[i_dir].num_point == 0 &&
                            cell_now.ary_cell_vertices[i_dir].num_point != 0){
                        //cells_glb.ary_cell_vertices[i_dir] = cells_now.ary_cell_vertices[i_dir];
                    }
                }

updateVertices的基本原理是新的信息与旧的信息的加权平均。
对于没有在全局地图中找到对应关系的local cell，则将其直接加入到全局地图中。整个过程中都是基于指针在操作。