鐳射SLAM簡單入門（1）-matlab讀取bag檔案並儲存為mat檔案
最近在一篇公眾號發現了一篇鐳射SLAM的入門文章，作者提供了原始碼，下載下來跑了一下，效果還不錯。之前一直在用鐳射雷達建圖，但一直沒能對鐳射雷達SLAM基礎理論有深入的研究，因此萌生了以該程式碼為基礎，系統整理鐳射雷達SLAM步驟的想法。原文見：
本系列主要內容是理解鐳射SLAM的過程，在原文的基礎上進行了改進和升級，並增加了的細節內容，同時也加入了部分自己的理解。
作者在文章中提供了所有的程式碼，把所有的程式碼複製到同一資料夾下，執行main。m檔案即可，我也將程式碼整理到我的github，下載地址為：
第一部分記錄matlab讀取bag檔案並將需要的資料儲存成mat檔案的過程。寫這部分的原因是網路上關於matlab讀取bag檔案的內容較少，在提取資料的過程中浪費了較多時間。主要步驟包括下載bag檔案，在ros環境下解壓縮，解壓縮後bag檔案的資料提取和儲存（matlab）。
1 下載bag檔案
谷歌的Cartographer演算法中提供的資料集，檔案下載地址：
我下載了b2-2014-12-12-14-41-29。bag，檔案大小為46MB。
ps：下載後直接在matlab下用rosbag函式讀取，會提示如下錯誤：
原因：matlab只能讀取未壓縮的檔案，直接下載的bag檔案是壓縮後的檔案，因此首先需要解壓縮。
2 在ros環境中解壓縮bag檔案
在ros環境（ubuntu16。04+ros kinetic）下對原始的bag檔案進行解壓縮，得到解壓後的bag檔案。
rosbag decompress b2-2014-12-12-14-41-29。bag
解壓後文件大小為230M：
3 在matlab下對bag檔案中的資料進行提取和儲存
matlab提供了ros相關的函式來提取資料，但是一次對所有的資料進行處理，會發生記憶體不足的問題。比如我用如下的程式碼讀取bag檔案：
bag = rosbag（‘b2-2014-12-12-14-41-29。bag’）；%讀取所有資料
%讀取水平雷達topic 資料
laser = select（bag， ‘Time’， 。。。
［bag。StartTime bag。EndTime］， ‘Topic’， ‘/horizontal_laser_2d’）；
x = readMessages（laser）；
matlab報錯（記憶體不足）：
Error in robotics。ros。BagSelection/readMessages （line 194）
msgs = obj。deserializeMessages（obj。MessageList， rows）；
Error in LoadMeasurements （line 47）
msgs = readMessages（bag）；
因此，採用迴圈的方式逐一讀取資料，程式碼如下：
clear；clc；
bag = rosbag（‘b2-2014-12-12-14-41-29。bag’）；%讀取所有資料
%讀取水平雷達topic 資料
laser = select（bag， ‘Time’， 。。。
［bag。StartTime bag。EndTime］， ‘Topic’， ‘/horizontal_laser_2d’）；
%% 從檔案中查詢資料的大小
N = laser。NumMessages；%雷達資料條數
x = readMessages（laser，1）；
［M，~］ = size（x{1，1}。Ranges）；
times = zeros（N，1）；%時間引數
ranges = zeros（N，M）；%距離引數
%% 迴圈讀取資料 ：整體讀取時會出現記憶體不足的情況
for i=1：N
temp = readMessages（laser，i）；
times（i） = temp{1，1}。Header。Stamp。Sec；%時間
ranges_temp = temp{1，1}。Ranges；%雷達測量（1079維資料）
for j = 1：M %不知道如何整體讀取，所以加了迴圈
laser_echo = ranges_temp（j，1）。Echoes；
［xx，yy］ = size（laser_echo）；
if xx*yy<1 %當laser_echo為空時，跳出當前迴圈
continue
end
ranges（i，j） = laser_echo（1）；%雷達測量的距離資料
end
%顯示進度
if mod（i，100）==0
disp（［‘處理進度%：’， num2str（i/N*100）］）；
end
end
%資料儲存為mat檔案
save new_laser_data。mat times ranges
以上三個步驟得到了從鐳射SLAM建圖需要的鐳射雷達ranges資料和times資料並儲存到。mat檔案中！
