tinySLAM代码逻辑结构

最新推荐文章于 2023-02-06 07:30:51 发布

LilyNothing

最新推荐文章于 2023-02-06 07:30:51 发布

阅读量5k

点赞数 8

分类专栏： SLAM学习

本文链接：https://blog.csdn.net/LilyNothing/article/details/62881142

版权

SLAM学习专栏收录该内容

11 篇文章 2 订阅

订阅专栏

tinySLAM的代码逻辑结构

前言&&吐槽：在这里先提前声明，我是在我自己理解的基础上写下这些东西的，如果我的理解有误，还是希望大神们能帮忙我解答一下，至少不要让我处于不明不白中。

上面说的是自己理解有误，对于tinySLAM这篇文章，但是如果没有理解有误的情况下，我要吐槽一下这篇文章了。下面是吐槽部分，大家可以忽略，但是如果你对于这篇文章也很不明白，那么欢迎来看看我的吐槽。

文章当中说了这是一个不超过两百行代码就能实现的SLAM，所以就叫了tinySLAM，我看了很多遍，然后发现这个tinySLAM的论文里面说了，它这两百行的代码并木有包括loop closing的内容，loop closing的算法还在他们的研究内容中（当然这话是2010年说得，我也不知道现在研究出啥了，不过看他们的tinyslam的代码，似乎是已经研究出来的。但是原谅我这个看源代码能力弱鸡的人，真是没看懂。）呐，这是文中的原句：
不包含loop closing
然后整篇文章读下来呢，我知道的信息就是这篇文章讲的不超过两百行的代码是它提供的代码文件下下的trunk->test->test_lab_reverse.c这个文件，以及trunk->CoreSLAM.c ,trunk->CoreSLAM.h下实现的几个函数，其实主要是后面的这两个文件实现了论文中所说的功能，test-lab_reverse.c这个文件只是调用了那两个文件中的函数，其实这个test_lab_reverse.c就是一个作者在写代码的时候调试所用的一个文件而已。至于其他的文件以及函数，我真是是没看懂，因为没注释，没相应的文章，看的真是头晕。虽然论文只写了那两个文件中的内容，但是他的代码运行的结果在我个人理解看来还是加入了闭环检测，因为我去查看了一下他的CMakelist.txt,同时结合了一下他的readme文件。Cmakelist中写的是将test_loop_closing.c进行编译链接生成readme文件中的test_SDL，也就是这个SLAM对应的运行文件。所以感觉这个运行效果和文章还是有些区别的。

以上就是吐槽结束了。那么对于这篇文章，我还是要把相应的代码逻辑在我个人的理解之下梳理一下的。以下就是正文啦！

正文来啦！

一、tinySLAM论文涉及部分代码的整体结构

针对于论文，我看了很多次代码，发现关于论文的那部分的代码是由test_lab_reverse.c这个文件串起来的。这个是一个测试的主函数，测试了论文中所讲的几个主要函数。这个test_lab_reverse.c文件的整个流程图如下：
tinySLAM流程图

图片说明：流程图中红色框的部分就是这篇论文的重点了，其中在蒙特卡洛算法中调用了函数ts_distance_scan_to_map这个函数，在ts_map_update中调用了另一个论文中的核心函数就是ts_map_laser_ray。所以论文的主要部分是红框当中的两个函数

二、流程图中的各个函数

2.1 read_sensor_data

这个函数的功能就是将文件中的传感器的数据转为scan点集数据，用相应的数据结构缓存起来。这个部分传感器的数据格式如下：

含义	符号
时间戳	timestamp	用于记录时间
里程计数据	q1,q2	用来表示当前移动的距离（相对于上一次）
机器人角度，速度	v,psidot	用于更正scan点集的位置信息（x,y,theta）
位置信息	position[3]	相对位置的记录，向前移动，向后移动，闭环 (三种情况)
是否有障碍物信息	d[TS_SCAN_SIZE]	记录激光是否扫到障碍物（hit点集或者是visit点集）

上面表格中的数据就是传感器的整个数据了，这个read_sensor_data就是将文件中保存的数据缓存到数据结构当中。

2.2 ts_map_init

这个函数的功能就是初始化map这个数据结构。map这个数据结构就是一个TS_MAP_SIZE*TS_MAP_SIZE的数组。初始化的值是设为(TS_OBSTACLE + TS_NO_OBSTACLE) / 2，这是两个常量值。

2.3 monte_carlo_move

这个函数是点集匹配的函数，就是找到最佳的匹配位置。返回的是最佳的匹配位置。这个函数采用了蒙特卡洛随机算法来找最佳位置。这个函数的流程图如下：(ps:画流程图差不多忘记，回头得去补补了。)
蒙特卡洛算法

以上就是通过蒙特卡洛算法随机找到最佳位置的算法，当然也不是那么随机，函数里面还是会基于之前找到的最佳位置来进行查找的。

2.4 ts_map_update

ts_map_update是更新地图的一个过程，实质上是更改地图上的值的过程，之后绘制地图的部分是使用一个Bresenham算法记性绘制的（这个算法无关紧要），重点是这个更新地图值的过程。下面给出ts_map_update函数的流程图：

上面流程图中的填充部分，那部分代码对应的就是论文中的下图：
论文中的图片

再把那部分代码贴一下：

    s = sin(pos->theta * M_PI / 180);
    x1 = (int)floor(pos->x * TS_MAP_SCALE + 0.5);
    y1 = (int)floor(pos->y * TS_MAP_SCALE + 0.5);
    // Translate and rotate scan to robot position
    for (i = 0; i != scan->nb_points; i++)
    {
        x2p = c * scan->x[i] - s * scan->y[i];
        y2p = s * scan->x[i] + c * scan->y[i];
        xp = (int)floor((pos->x + x2p) * TS_MAP_SCALE + 0.5);//scan点集的全局位置
        yp = (int)floor((pos->y + y2p) * TS_MAP_SCALE + 0.5);
        dist = sqrt(x2p * x2p + y2p * y2p);//scan点集相对于机器人的位置的距离
        add = hole_width / 2 / dist;
        x2p *= TS_MAP_SCALE * (1 + add);
        y2p *= TS_MAP_SCALE * (1 + add);
        x2 = (int)floor(pos->x * TS_MAP_SCALE + x2p + 0.5);//在一个圆的范围内？
        y2 = (int)floor(pos->y * TS_MAP_SCALE + y2p + 0.5);
        if (scan->value[i] == TS_NO_OBSTACLE)
        {
            q = quality / 4;
            value = TS_NO_OBSTACLE;
        }
        else
        {
            q = quality;
            value = TS_OBSTACLE;
        }
        //printf("%d %d %d %d %d %d %d\n", i, x1, y1, x2, y2, xp, yp);
        ts_map_laser_ray(map, x1, y1, x2, y2, xp, yp, value, q);//x1,y1是机器人的位置，x2,y2是已这个点为中心，然后以与hole_width有关的半径范围内找的一个点吗？，xp，yp是scan点集中的一个点的位置
    }

其中的（x1,y1）(x2,y2)，(xp,yp)这些坐标和上图中的坐标是一一对应的。其中的（x2,y2）是通过（xp，yp）加上hole_width得到的。对每个hit点来说，周围会有一个类似于V字型的洞。

ts_map_update里面会用到一个函数ts_map_laser_ray这个函数，这个函数的作用就是将scan的点整合到地图中去，所谓整合其实就是修改hit点对应的map中的value值。

2.5 ts_map_laser_ray

关于ts_map_laser_ray这个函数，还是需要说明一下，这个函数看起来前面有很多If条件的判断，其实就是不断判断溢出情况的过程。因为画在地图上不是单独画一个obstacle point，而是画一个hole，因此求出一个hit点（xp，yp）之后，还需要加上hole_width得出（x2，y2），那么这个（x2，y2）可能会超过map的范围，所以对于超过map范围的点，用几个if条件将其映射到四个边界线上。对于scan点集对应的map点上value值的修改，以下代码就是修改的操作了。

 // Integration into the map
        *ptr = ((256 - alpha) * (*ptr) + alpha * pixval) >> 8; //对这个位置的值进行修改。
        if (error > 0)
        {
            ptr += incptry;
            error += diago;
        }
        else
            error += horiz;

以上就是ts_map_laser_ray函数部分的理解啦！具体的一些变量啥的，真心不是很清楚它的含义。感觉就是绕着一个圆不断的在调整调整，只要是那个hit点在的那个圆内的点的value值都需要修改，毕竟画出来是一个hole。

2.6 about others(其它函数)

之后的函数就是就是画图的过程了（map中不同的value对应不同的情况，比如value大的值就是hit点的位置，将其加深颜色之类的，这些就是绘图的问题啦。）其实这个函数的clipping部分没怎么看懂，就是处理各种特殊情况的部分没有看懂。所以还得多花时间在这上面呀！fighting~

以上，就是我对于这篇论文涉及到的代码的理解了。还有就是test_loop_closing.c这个文件内其实就是加了SDL绘制图像，以及加了绕几圈的圈数，并没有加入闭环检测的函数。

我的想法：对于一个hit点，不是单画一个点，而是一个洞的目的就是为了让误差模糊化，这个洞的大小这个值也可以让机器人在行走的过程中对这个值进行设置，比如说设置一下，让机器人的行走范围加减这个洞的半径，然后这样就不会撞墙了。以上纯属个人想法。

三、一些问题的个人理解以及来自同学的各种“质问”

问：论文中采用的是蒙特卡洛算法实现的scan与map的匹配，匹配的过程是怎样的流程呢？
答：蒙特卡特算法是随机的机器人的位置（小鸟），然后找到这个位置摄像头扫描得到的laser数据转换得到scan点集。求出这个scan点集的全局坐标之后，看它落在map上的哪些位置，而map上的每个点都有一个value值，求出这个scan点集落在map上的点的value的总和，然后除以点的个数（相当于求出这个scan点集落在这个map上的一个score），这样就可以比较这个score来判断这个位置是好是坏。那么第一帧数据没有匹配的对象，直接在map上修改第一帧scan点集落在的位置上的value值，便于之后的匹配，每匹配一帧，就是修改map上面一些点的value值，那么之后就可以知道哪个位置是较为准确的。

PS:以下的问题是同学问我的问题，都一一列出来解答一下吧！感谢小伙伴的问题，让我更加深刻的理解SLAM。

问：一个position对应的scan点集是通过laser传感器转换来的吗？
答：是的。因为scan点集（laser数据）和机器人的位置是一一对应的。其实SLAM的过程就是不断的将scan点集与已有的map进行匹配，匹配完成之后再修改这个scan点集对应的机器人的位置，这就达到了校准机器人位置的目的。

问：与已有地图的匹配是怎么进行的？也就是匹配算法的实现？
答：关于匹配算法，论文中提到采用的是Monte-Carlo算法，这个算法的思想如下：
随机找一个位置（机器人的位置，这个位置其实也不是随便找的，而是基于之前找到的一些较优的位置的基础上进行查找的），然后求出这个位置对应下的scan点集与map的匹配程度，迭代最多1000次之后，返回匹配度最高的那个位置。

问：scan点集与已有地图的匹配程度是怎么衡量的？
答：论文中采用的地图是一个grey-level的map，在代码中采用的是一个一维数组 map[TS_MAP_SIZE * TS_MAP_SIZE]来存储map中对应的每一个点的value值，匹配程度就是通过计算scan点集落在map上相对应点的value值的平均值来进行判断的。而map中的每个点的value值会在每次scan点集匹配完成，地图更新的时候做出修改，修改的目的是为了之后进行更好的匹配，使得匹配度能够得到更好的衡量。

欧了~终于写完啦！！！欢迎大家来吐槽吐槽哈！写的有点凌乱，但是：书读百遍，其义自见。那就多读几遍好啦！

LilyNothing

关注

8
点赞
踩
11

收藏

觉得还不错? 一键收藏
3
评论
tinySLAM代码逻辑结构

tinySLAM的代码逻辑结构前言&&吐槽：在这里先提前声明，我是在我自己理解的基础上写下这些东西的，如果我的理解有误，还是希望大神们能帮忙我解答一下，至少不要让我处于不明不白中。上面说的是自己理解有误，对于tinySLAM这篇文章，但是如果没有理解有误的情况下，我要吐槽一下这篇文章了。下面是吐槽部分，大家可以忽略，但是如果你对于这篇文章也很不明白，那么欢迎来看看我的吐槽。文章当中说了这是一个不超过
复制链接

扫一扫