激光 SLAM 建图算法代码实践

本文是我在学习深蓝学院的激光slam课程第七次实践的笔记。本次实践主要是熟悉并手写三类建图方法，分别是：覆盖栅格建图算法、计数建图算法和 TSDF 建图法。

覆盖栅格建图算法

补充代码,通过覆盖栅格建图算法进行栅格地图构建

代码文件结构

老规矩还是先熟悉代码文件结构。项目结构比较简单：

OccupancyMappingProject
└── src
    ├── data------------------------------------数据文件夹，包括机器人位姿和激光雷达观测数据
    │   ├── pose.txt----------------------------机器人位姿
    │   ├── ranges.txt--------------------------激光雷达每一帧中每一束激光的观测距离
    │   └── scanAngles.txt----------------------激光雷达每一帧中每一束激光的角度距离
    └── occupancy_mapping-----------------------建图Package
        ├── CMakeLists.txt
        ├── include-----------------------------头文件
        │   └── occupancy_mapping
        │       ├── occupancy_mapping.h---------主要声明一些全局变量作为建图的参数
        │       └── readfile.h------------------声明读取文件相关函数
        ├── package.xml
        └── src---------------------------------源代码
            ├── occupancy_mapping.cpp-----------建图相关以及发布地图
            └── readfile.cpp

下面来看一下主要文件的结构和作用：

readfile.cpp

这个文件中的函数都比较简单，主要包含三个函数：
- stringToNum()：数字字符串转成数字类型（string->int/float/double）
- NormalizationAngle()：角度正则化到 [-pi, pi] 之间
- splitString()：根据给定的分隔符和一行字符串分割成多个所需字符串
- ReadPoseInformation()：读取机器人位姿信息，每个位姿信息包括三个参数：x,y,theta，其中角度需要正则化
- ReadLaserScanInformation():读取激光测距和角度信息
occupancy_mapping.cpp
- 建图相关：
  - TraceLine()：计算并返回两个栅格连线之间经过的栅格，结果不包含(x1, y1)，可能包括 (x0, y0)
  - SetMapParams():初始化地图以及三种建图算法的相关参数
  - GridIndex ConvertWorld2GridIndex()：从世界坐标系转换到栅格坐标系
  - GridIndexToLinearIndex()：二维栅格坐标转为线性坐标
  - isValidGridIndex()：判断index是否有效
  - DestoryMap()：清理地图占用内存
  - OccupancyMapping()：给定所有激光数据和相应机器人位姿，更新地图
- ROS 相关：
  - PublishMap()：发布地图
建图过程比较简单：
- 初始化 node 节点和类型为 nav_msgs::OccupancyGrid 的 publisher 用于发布地图
- 定义数据文件的路径并进行数据读取
- 初始化地图信息
- 进行建图
- 发布地图

以上就是这个项目的文件结构，核心函数为 OccupancyMapping()，代码中已经提供了将激光数据转化为世界坐标的过程，我们只需要补充：给定激光束击中的栅格和机器人所在的栅格，进行地图相关栅格的更新即可：

// TODO
// 找到激光束击中的栅格坐标
GridIndex end_grid_index = ConvertWorld2GridIndex(world_x, world_y);

auto updateGrid = [](const GridIndex& index, double update_value) -> void {
    // convert pMap to log-odd then update
    int linear_grid_index = GridIndexToLinearIndex(index);
    double px = (double) pMap[linear_grid_index] / 100.0;
    double lx = std::log(px / (1 - px));
    lx += (update_value);
    
    // convert log-odd back to pMap
    pMap[linear_grid_index] = 100 * (1.0 - (1.0 / (1 + std::exp(lx))));
    pMap[linear_grid_index] = std::max(mapParams.log_min, std::min(mapParams.log_max, (double)pMap[linear_grid_index]));
};

// update end grid occupied by lidar beam with log-odds of occupation
if (isValidGridIndex(end_grid_index)) {
    int linear_grid_index = GridIndexToLinearIndex(end_grid_index);
    updateGrid(end_grid_index, mapParams.log_occ);
}

// update grid that are passed through by lidar beam with log-odds of free
std::vector<GridIndex> free_grid_indices = TraceLine(robot_index.x, robot_index.y, end_grid_index.x, end_grid_index.y);
for (const GridIndex& grid_index: free_grid_indices) {
    if (isValidGridIndex(grid_index)) {
        updateGrid(grid_index, mapParams.log_free);
    }
}

理论部分参考：基于已知位姿的栅格构图算法，这里主要注意一下 pMap 存储的是 $P(m)$，而我们在更新过程中使用的参数是 $l(m)$，中间由一个转换关系：$l(x) = \log{\frac{p(x)}{1 - p(x)}}$

计数建图算法

将第 1 题代码改为通过计数建图算法进行栅格地图构建

每一帧的更新栅格击中和穿过的次数：

// 找到激光束击中的栅格坐标
GridIndex end_grid_index = ConvertWorld2GridIndex(world_x, world_y);

if (isValidGridIndex(end_grid_index)) {
    int linear_grid_index = GridIndexToLinearIndex(end_grid_index);
    pMapHits[linear_grid_index]++;
}

// update grid that are passed through by lidar beam with log-odds of free
std::vector<GridIndex> free_grid_indices = TraceLine(robot_index.x, robot_index.y, end_grid_index.x, end_grid_index.y);
for (const GridIndex& grid_index: free_grid_indices) {
    if (isValidGridIndex(grid_index)) {
        int linear_grid_index = GridIndexToLinearIndex(grid_index);
        pMapMisses[linear_grid_index]++;
    }
}

遍历完所有帧之后对地图栅格的更新：

for (size_t i = 0; i < mapParams.height * mapParams.width; ++i) {
    // Count mapping final update
    if (pMapMisses[i] + pMapHits[i] != 0) {
        pMap[i] = 100 * static_cast<int>(static_cast<double>(pMapHits[i]) /static_cast<double>(pMapMisses[i] + pMapHits[i]));
    }
}

过程比较简单

TSDF 建图法

将第 1 题代码改为通过 TSDF 建图算法进行栅格地图构建

每一帧的更新栅格的 TSDF 值和相应的权重：


// number of grids surronding by the end grid index that we update
static const int nums_surronding_grids = 10;
double robot_laser_x = end_grid_index.x - robot_index.x;
double robot_laser_y = end_grid_index.y - robot_index.y;

for (int i = -nums_surronding_grids; i <= nums_surronding_grids; ++i) {
    GridIndex index = end_grid_index;
    double dx = 0;
    double dy = 0;
    
    if (robot_laser_x == 0) {
        // dx = 0;
        dy = i;
    } else if (robot_laser_y == 0) {
        // dy = i;
        dx = i;
    } else {
        dx = i;
        dy = i * std::fabs(robot_laser_y / robot_laser_x);
    }

    index.x += dx;
    index.y += dy;

    if (!isValidGridIndex(index)) {
        continue;
    }

    double grid_robot_dist = std::sqrt(std::pow(index.x - robot_index.x, 2) + std::pow(index.y - robot_index.y, 2));
    grid_robot_dist *= mapParams.resolution;
    double sdf = laser_dist - grid_robot_dist;
    double tsdf = std::min(1.0, std::max(-1.0, sdf / 0.1));

    int linear_grid_index = GridIndexToLinearIndex(index);

    // use 1.0 as weight
    pMapTSDF[linear_grid_index] = (pMapW[linear_grid_index] * pMapTSDF[linear_grid_index] + 1.0 * tsdf) / (1.0 + pMapW[linear_grid_index]);
    pMapW[linear_grid_index] += 1.0;
}

TSDF 算法的基本过程是对一定范围的栅格计算 TSDF 值，利用 TSDF 值对栅格的占据概率进行更新，这里我用的方法是：

沿激光束方向在击中的栅格周围遍历一定数量（这是是前后各 10 个）栅格，对每个栅格：
- 计算 sdf 值和 tdsf 值
- 更新其栅格的 TDSF 值，单次测量权重设置为 1.0

遍历完所有帧之后对地图栅格的更新：

for (size_t i = 0; i < mapParams.height; ++i) {
        double prev_tsdf = pMapTSDF[i * mapParams.width];
        for (size_t j = 1; j < mapParams.width; ++j) {
            double curr_tsdf = pMapTSDF[i * mapParams.width + j];

            // mark pMap as free/unknown/occupied based on sign of TSDF value:
            // 1. negative -> unknown
            // 2. positive -> free
            // 3. exact zero(rarely happens!) -> occupied
            if (curr_tsdf > 0) {
                pMap[i * mapParams.width + j] = 0;
            } else if (curr_tsdf < 0) {
                pMapW[i * mapParams.width + j] = -1;
            } else {
                pMapW[i * mapParams.width + j] = 100;
            }

            // linear interpolation to find the grid where 0 tsdf lies on
            if (prev_tsdf * curr_tsdf < 0) {
                if (std::fabs(prev_tsdf) > std::fabs(curr_tsdf)) {
                    pMap[i * mapParams.width + j] = 100;
                } else {
                    pMap[i * mapParams.width + j - 1] = 100;
                }
            }

            prev_tsdf = curr_tsdf;
        }
    }

由于 TSDF 建图算法着重于找到曲面（障碍物）而没有特地区分空闲的区域，这里我更新栅格占据概率的方法是：

沿地图每一行栅格进行遍历，每一个栅格：

首先判断 TSDF 值，根据其符号判断空闲/未知/占据：
- 大于0：空闲
- 小于0：未知
- （理论上有可能，实际基本不可能出现）等于 0：占据
此外如果同一行中前一个栅格符号和当前栅格 TSDF 值的符号不同，则根据前后两个栅格 TSDF 值距离 0 的大小来判断 0 会落在哪个栅格里，并将该栅格设置为占据

结果

结果-覆盖栅格建图算法

大致能反映环境状态，但是有一部分障碍物不太明显，考虑有可能是实际中由于误差所以实际障碍物栅格被击中的次数较少，而穿过的次数较多因此被稀疏，提高更新占据的对数概率值可以缓解

结果-计数建图算法

大致能反映环境状态，但是大部分障碍物不太明显，理由同上

结果-TSDF 建图法

TSDF 场

地图

通过比较可以发现， TSDF 可以正确标志出大部分障碍物的位置，而且厚度都在一个栅格之内，但由于其更新方法只依靠左右两侧的栅格，所以在某些扫描数据比较少的地方会错误地标志障碍物（地图的右下角），噪声较多