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

前言

本节继续对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;
}

流程如下

1. 对于当前栅格集合中每一个包含了足够多的点的栅格，根据其hash索引找到其在全局地图中对应的栅格
2. 如果能找到，那么将两个来自全局和局部地图的相对应的栅格存储在overlap_ship的两个成员变量中
3. 否则，就是当前的有效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的基本原理是新的信息与旧的信息的加权平均。其函数如下

void Cell::updateVertices(Cell & cell_new, enum Direction update_direction){
    //update the vertices of a local surface (with certain prediction direction) inside a cell
    ROS_DEBUG("updateVertices");
    //init
    //double dyna_thr_grid_ratio = 0.5;
    int num_test = param.num_test*param.num_test;
    double variance_map_update = param.variance_map_update;
    Eigen::Matrix<double, 1, Eigen::Dynamic> update, observe, variance_update ,variance_observe;
     update = observe = variance_update = variance_observe = Eigen::MatrixXd::Zero(1, num_test);

    PointMatrix & points_old = ary_cell_vertices[update_direction];
    PointMatrix & points_new = cell_new.ary_cell_vertices[update_direction];

     update = points_old.point.leftCols(num_test).row(update_direction);
     observe = points_new.point.leftCols(num_test).row(update_direction);
     variance_update = points_old.variance.leftCols(num_test);
     variance_observe = points_new.variance.leftCols(num_test);

    //iteratively least square update
    for(int i = 0; i < num_test; i++){
        if((variance_update(0, i) <= variance_map_update) && (variance_observe(0, i) <= variance_map_update)){
            update(0, i)   = (update(0, i) * variance_observe(0, i) + observe(0, i) * variance_update(0, i))
                             / (variance_update(0, i) + variance_observe(0, i));
            variance_update(0, i) = variance_update(0, i) * variance_observe(0, i) / (variance_update(0, i) + variance_observe(0, i));
        }
        else if((variance_update(0, i) > variance_map_update) || (variance_observe(0, i) > variance_map_update)){
            update(0, i)   = (variance_update(0, i) <= variance_observe(0, i)) ? (update  (0, i)) : (observe (0, i));
            variance_update(0, i) = (variance_update(0, i) <= variance_observe(0, i)) ? (variance_update(0, i)) : (variance_observe(0, i));
        }
    }
    updated_times[update_direction] ++;

     points_old.point.leftCols(num_test).row(update_direction) = update;
     points_old.variance.leftCols(num_test) = variance_update;

     points_old.variance_sum = points_old.variance.leftCols(num_test).sum();
}

其中，需要注意的是原代码中使用了num_test作为更新的上界范围，其实是num_test_square,看代码的时候差点没注意。
对于没有在全局地图中找到对应关系的local cell，则将其直接加入到全局地图中。整个过程中都是基于指针在操作。