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，用於自動檢測閉環並且更正機器人的軌跡（位置）。
以上就是這篇文章的一個整體概括了，我個人覺得第三部分真的有點暈，還有就是論文中寫的東西確實很少，閉環檢測這些也並木有實現，其實就是和目前我們項目實現的工作差不多，只是實現了匹配，還未實現閉環檢測。因爲還沒完全弄懂，所以還是得去看看代碼怎麼實現的。
接下來將會對代碼進行一下閱讀，然後瞭解代碼的邏輯結構。