tinySLAM的代碼邏輯結構
前言&&吐槽：在這裏先提前聲明，我是在我自己理解的基礎上寫下這些東西的，如果我的理解有誤，還是希望大神們能幫忙我解答一下，至少不要讓我處於不明不白中。
上面說的是自己理解有誤，對於tinySLAM這篇文章，但是如果沒有理解有誤的情況下，我要吐槽一下這篇文章了。下面是吐槽部分，大家可以忽略，但是如果你對於這篇文章也很不明白，那麼歡迎來看看我的吐槽。
文章當中說了這是一個不超過兩百行代碼就能實現的SLAM，所以就叫了tinySLAM，我看了很多遍，然後發現這個tinySLAM的論文裏面說了，它這兩百行的代碼並木有包括loop closing的內容，loop closing的算法還在他們的研究內容中（當然這話是2010年說得，我也不知道現在研究出啥了，不過看他們的tinyslam的代碼，似乎是已經研究出來的。但是原諒我這個看源代碼能力弱雞的人，真是沒看懂。）吶，這是文中的原句：
然後整篇文章讀下來呢，我知道的信息就是這篇文章講的不超過兩百行的代碼是它提供的代碼文件下下的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文件的整個流程圖如下：
圖片說明：流程圖中紅色框的部分就是這篇論文的重點了，其中在蒙特卡洛算法中調用了函數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點集匹配完成，地圖更新的時候做出修改，修改的目的是爲了之後進行更好的匹配，使得匹配度能夠得到更好的衡量。
歐了~終於寫完啦！！！歡迎大家來吐槽吐槽哈！寫的有點凌亂，但是：書讀百遍，其義自見。那就多讀幾遍好啦！