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