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编写的一些准备工做就都已经完成了，下一篇文章将要对算法部分进行实现。