tinySLAM阅读笔记
初衷：tinySLAM是openSLAM中实现最为简单的一个SLAM方法，相比于ORB-SLAM之类的，这个代码的核心实现没有超过两百行，所以还是相对简单一些，对于我这样一个新手，拿来入门，深刻理解SLAM的工作原理，还是很有帮助的。我的导师希望我把这篇文章2010年发表在ICARCV的论文（CoreSLAM:a SLAM Algorithm in less than 200 lines of C code）以及对应的代码都熟悉掌握，这样能深刻理解SLAM。所以就来深入研究研究一下好啦！
摘要
主要是基于particle-filter进行的。SLAM的主要实现方法有三种，如下图：
其中粒子滤波的基本思想是：通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。这篇文章是基于蒙特卡洛的这种粒子滤波的方法。具体的实现方法之后代码部分具体学习。
主要想法是基于粒子滤波器，将 激光的信息整合到定位系统中去。
一、介绍
简单的介绍了tinySLAM的优点及其名字的由来，同时也简单对整篇文章的布局框架进行了介绍。重点是在第三部分METHODOLOGY,主要介绍了tinySLAM的实现思想。
二、之前的工作
这部分简单介绍了SLAM的分类以及相关的文章。SLAM的分类如下：
- 在计算机视觉方面，根据是一个camera还是多个camera，得出算法（视觉SLAM）
- 使用激光（laser）sensor或者声呐定位器
- 使用Kalman过滤器（根据计算的method）
- 使用particle filter 或者Rao-Blackwellized particle filters（这是 particle and Kalman的结合，比如 FastSLAM）
同时还各种说明这篇文章优于其它文章的部分，这是论文中不可避免的部分，具体就大家自己去看啦。
三、具体方法（哼哼哼，敲黑板，重点来啦！！）
不同于DP-SLAM的每个particle filter对应一个map，然后最后将这些map结合起来（个人觉得类似于子图的概念），但是不能实现闭环检测。tinySLAM只使用一张图，但是能实现闭环检测（虽然它的这种只使用一个map的思想并不是 map-per-particle的目的，但是这篇文章的目的是实现简单计算以及是闭环尽可能误差小）。
CoreSLAM（tinySLAM）主要思想就是基于粒子滤波器（particle filter）将激光数据整合到定位子系统中。这其中涉及到以下两个主要函数：
- ts_distance_scan_to_map :这个函数的输入是这一帧的点集，以及已有的地图，还有当前随机取的一个位置（测试这个位置是否是最优位置）。输出是这个位置的似然度，然后其它函数根据这个似然度来判断最优位置。在这个函数内部，是先将一帧的点集转为相对于当前position的点集，然后再进行似然度计算。这个似然度的计算仅仅是通过将hit点集的map value值加起来求平均值。
- map update function：用来在机器人行走的过程中进行地图的更新。接下来再讨论细节。
对于构建一个与粒子滤波器相容的地图是不简单的，所以采用一个grey-level maps，这个地图的更新是根据似然度的峰值，然后来画一个hole，这个hole的宽度就是这个位置处的似然度。对于每个hit点，并不是简单的画一个点。对于将其整合到map中的是通过
ts_map_update这个函数还调用了另一个函数ts_map_laser_ray。使用的是Bresenham算法将laser rays绘制到地图中，同时内部使用一个加强版的bresenham算法计算正确的侧面（profile？？）。一步执行一次，由于高分辨率（每个像素是1cm）所以速度还是比较快的。
CoreSlam还是很容易整合到粒子滤波器中的，但是也可以不用整合。事实上，这个构建的地图（包含一些洞和斜坡的图），也能和任何的梯度函数进行使用。使用hole function，能够使得匹配函数能容易收敛。这篇文章的单机版本使用的是简单的Monte-Carto算法将当前的点集与已有地图进行匹配，并且得到机器人应该更新的位置。事实上，这个单机版本是为了调整CoreSLAM的参数而建立的。例如参数点集整合到map中的速度（在ts_map_update函数中的quality参数），被设置到50，同时围绕着hit点的‘hole’的宽度设置为一个固定的值600mm，同时地图的度量是1个point=1 cm。在单机版本中，里程计可以被忽略或者被用来作为蒙特卡洛方法方法的初始点。
基于particle filter的版本必须解决的问题是模棱两可的位置问题，因此就会涉及到重定位的问题。（当我们从一幅已经构建好的图而不是空图开始的时候。 因此test_lab_reverse.c只是测试从一幅空图开始的？？？）最好的部分是里程计的整合，因为这样能够处理非系统误差，比如滑动。
粒子滤波器不仅仅对于整合传感器，里程计，激光是一个非常好的框架，同时也可以整合GPS和指南针数据。（虽然还没success）。
这篇文章将会通过讨论两个问题来呈现CoreSLAM。
- 定位的准确度（accuracy of localization）
- 整合数据到地图中的一个延迟（the latency of integration of the data into the map）
subpixel的accuracy：尽管这篇文章的里面我们的地图的分辨率是是1cm，但是还是可能会测量到小于1cm的情况，因此函数ts_distance_scan_to_map 会考虑将几个点一起进行计算（就是将一个scan点集的所有hit点一起计算）。因为有一些激光点会落在地图上的其它点上，所有即使是1mm的移动也会被测量到。
延迟（latency）：这里延迟的概念就是scan点集整合到map中的时间。这个延迟需要去测量相对于map分辨率的小位移。比如：为了正确的测量一个以1cm/s的速度在1cm分辨率，frequency为1hz的地图上移动的物体的位移，需要等待10 measurements，延迟就应该是10。事实上这个延迟计算公式如下：
考虑到机器人的以及给出地图的分辨率，这个公式就会是如下情况：
文中还讲了闭环检测，但是代码木有实现闭环检测（也是简直了，问了写论文真的好拼），但是他能和任何有闭环检测的算法进行整合。事实上，作者他们正在为其进行闭环检测，但是还是在进行中。嗯~~~~
四、平台描述（就是各种硬件情况）
这篇文章基于的一些硬件条件如下：
- 代码部署平台：基于他们自己的robotic 平台——MinesRover，a homebrew robot developed jointly by Mines ParisTech and SAGEM DS.
- 测试机器：six wheels robot(四个驱动轮，两个odometry轮（中间）) fully equipped with snesors,including a Hokuyo URG04 laser scanner.
- 激光传感器：Hokuyo URG04 offers a 10HZ horizontal scanning，with a range of 5.6 meters.
- IMU: 5-axis, provides yaw rate information and the inclination of robot.
- 位置测量仪器：combine the results of compass and GPS（声呐测量）。结合激光传感器和声呐传感器。
需要说明的是这个激光传感器有两大缺点：
- 一是他的最大范围限制在5.6m
- 二是测量的频率也受限制（10hz）
五、结果
至于结果啥的，大家可以自己去看。我就不具体描述了。
六、总结
这篇文章就是建立一个简单并且有效的算法，这个算法使用的是激光数据进行SLAM，它的能够被很容易并且有效的整合到任何基于 粒子滤波器的定位系统中去。
文中提到作者们接下来的目标就是建立一个tinyLoopCloser，用于自动检测闭环并且更正机器人的轨迹（位置）。
以上就是这篇文章的一个整体概括了，我个人觉得第三部分真的有点晕，还有就是论文中写的东西确实很少，闭环检测这些也并木有实现，其实就是和目前我们项目实现的工作差不多，只是实现了匹配，还未实现闭环检测。因为还没完全弄懂，所以还是得去看看代码怎么实现的。
接下来将会对代码进行一下阅读，然后了解代码的逻辑结构。