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編寫的一些准備工作就都已經完成了，下一篇文章將要對算法部分進行實現。