HectorSLAM論文解析・代碼重寫（3）

這篇文章爲HectorSLAM系列的如下部分

• HectorSLAM的總體邏輯
  • 激光匹配
  • 地圖構造
  • 地圖更新
• 500行代碼重寫一個LidarSLAM
  • 測試數據的準備，和測試數據讀取模塊的編寫
  • GUI編寫
  • 地圖模塊的編寫
  • 核心模塊的編寫
    
    • 主循環
    • 匹配算法

 

 

本文中的代碼和數據放在：https://github.com/scomup/lslam

在【HectorSLAM論文解析・代碼重寫（2）】中，咱們已已經弄清楚了理論。從這一篇文章開始，咱們就用python一點一點的把前面的內容用代碼實現。

要注意的是，個人SLAM並非徹底照搬HectorSLAM的邏輯，增長了對車輪里程計的支持，還有一些我本身對Hector的優化。

 

固然在代碼以前，咱們還先要有測試用的數據，若是有手上有LiDAR和機器人固然能夠用真實數據。這裏我用gazebo的irobot create增長一個lidar，從gazebo環境中獲取實驗用數據。獲取的數據用rosbag record記錄爲bag文件，供反覆使用。

測試用的bag文件須要包含scan和odom信息。我把我錄製好的bag放在如下連接

https://github.com/scomup/lslam/blob/master/h1.bag

gazebo測試環境的搭建不是本文重點，因此不詳盡闡述。

 

1.測試數據讀取模塊

第一個模塊負責數據的讀取。擁有4個參數，bagfile指定bag文件名，scan_topic指定激光數據的topic名，odom_topic爲車輪里程計的topic名。start_time, end_time分別是bag文件的開始時間和終了時間。

 

class BagReader:
    def __init__(self, bagfile, scan_topic, odom_topic, start_time, end_time):
        self.scan_topic = scan_topic
        self.odom_topic = odom_topic
        self.start_time = start_time
        self.end_time = end_time
        self.points = []
        self.odoms = []
        self.data = []
        print "Bag file reading..."
        self.bag = rosbag.Bag(bagfile, 'r')
        print "Scan data reading..."
        self.readscan()
        print "Odom data reading..."
        self.readodom()
        print "Data sync..."
        self.sync()
        print "All ready."
        self.bag.close()

 

這個模塊擁有3個函數，readscan負責讀取雷達信息，readodom負責讀取車輪里程計信息，sync負責對時間戳的同步。

激光信息讀取函數的實現：

    def readscan(self):
        laser_projector = LaserProjection()
        for topic, msg, time_stamp in self.bag.read_messages(topics=[self.scan_topic]):
            cloud = laser_projector.projectLaser(msg)
            frame_points = np.zeros([0,2])
            for p in pc2.read_points(cloud, field_names=("x", "y", "z"), skip_nans=True):
                    p2d = np.array([p[0], p[1]])
                    frame_points = np.vstack([frame_points, p2d])
            self.points.append([time_stamp,frame_points])        

讀取激光信息的時候咱們用到了ROS的LaserProjection，把激光數據轉換爲pointcloud，再把全部的pointcloud儲存爲N×2的二維數組。在這個二維數組中，橫軸表明x座標和y座標，豎軸表明每個激光數據。而後再添上時間戳，儲存起來。

 

車輪里程計信息讀取函數的實現：

    def readodom(self):
        laser_projector = LaserProjection()
        for topic, msg, time_stamp in self.bag.read_messages(topics=[self.odom_topic]):
            qx = msg.pose.pose.orientation.x
            qy = msg.pose.pose.orientation.y
            qz = msg.pose.pose.orientation.z
            qw = msg.pose.pose.orientation.w
            t = tf.transformations.quaternion_matrix((qx,qy,qz,qw))
            t[0,3] = msg.pose.pose.position.x
            t[1,3] = msg.pose.pose.position.y
            t[2,3] = msg.pose.pose.position.z
       self.odoms.append([time_stamp,t])

讀取車輪里程計信息時，爲了方便之後作旋轉平移的變化，咱們直接把車輪里程計信息用一個4×4的旋轉平移矩陣代替。這裏的轉換直接用ros的tf完成。

 

車輪里程計和激光數據須要時間戳同步後才能使用，下面的代碼給出了一個簡單的時間戳同步的實現：

    def sync(self):
        idx = 0
        start_time =self.points[0][0] + rospy.Duration(self.start_time, 0)
        end_time =self.points[0][0] + rospy.Duration(self.end_time, 0)
        for time_stamp_scan,scan_data in self.points:
            if time_stamp_scan > end_time:
                    break
            if time_stamp_scan < start_time:
                continue
            time_stamp_odom,odom_data = self.odoms[idx]
            while idx < len(self.odoms) - 1:
                if time_stamp_odom > time_stamp_scan:
                    break
                time_stamp_odom,odom_data = self.odoms[idx]
                idx+=1
        self.data.append((scan_data,odom_data))

至此數據讀取模塊已經完成，爲了確認他是否良好工做，我又添加了一些測試代碼，把獲取的激光數據根據車輪里程計生成的旋轉平移矩陣轉換到正確的位置後，再用mathplotlib輸出顯示，顯示效果以下：

如上圖顯示，激光數據和里程計信息已經成功讀取。

 

1.GUI編寫

 在開始SLAM的實現前，咱們須要一套GUI來方便咱們查看，程序的工做狀況和debug。

功能要有

• 顯示地圖數據
• 顯示地圖中障礙物的機率數據
• 顯示機器人位置
• 顯示激光輸入
• 控制按鈕（bag文件的前進，後退，連續播放等等）

很遺憾這些功能ROS經常使用的rviz也不能所有實現，因此我用Qt作了一套專用的GUI。Qt方面也不是本文重點，因此只給出GUI效果圖，具體實現能夠看代碼。

 

上窗口顯示地圖和顯示機器人位置激光信息，下窗口顯示地圖障礙物機率，最下是一排控制按鈕。

 

至此SLAM編寫的一些準備工做就都已經完成了，下一篇文章將要對算法部分進行實現。