SLAM Benchmarking-佛莱堡 SLAM 数据集下载和误差工具使用教程

1、数据集的下载

官方数据集下载地址：http://ais.informatik.uni-freiburg.de/slamevaluation/datasets.php

下载步骤

下载步骤参考Miaowaaa博主写的《Karto_slam跑激光雷达(北阳ust-10lx下一篇介绍使用)》这篇文章。

1.首先在网站数据集页面中，可以看到多个地图，选择一个地图点击“download log file”

2. 你将看到一堆数据，等待网站加载完毕，确保数据集完整

3.然后全选复制（ctrl a，ctrl c）

4. 最后新建一个文档，后缀名为.clf，然后将数据粘贴进去

数据集转化

到这里我们算是把数据集保存下来了，那么接下来我们需要将其转化为ROS可以使用的bag文件，然后通过rosbag play来使用数据集，下面给出转化所需的python代码:

#!/usr/bin/env python
#coding=utf8
'''This is a converter for the Intel Research Lab SLAM dataset
   ( http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets/intel.clf )
   to rosbag'''

import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf
import sys

def make_tf_msg(x, y, theta, t,base,base0):
    trans = TransformStamped()
    trans.header.stamp = t
    trans.header.frame_id = base
    trans.child_frame_id = base0
    trans.transform.translation.x = x
    trans.transform.translation.y = y
    q = tf.transformations.quaternion_from_euler(0, 0, theta)
    trans.transform.rotation.x = q[0]
    trans.transform.rotation.y = q[1]
    trans.transform.rotation.z = q[2]
    trans.transform.rotation.w = q[3]

    msg = TFMessage()
    msg.transforms.append(trans)
    return msg
if __name__ == "__main__":
    if len(sys.argv) < 3:
        print "请输入dataset文件名。" 
        exit()
    print "正在处理"   sys.argv[1]   "..."
    with open(sys.argv[1]) as dataset:
        with rosbag.Bag(sys.argv[2], 'w') as bag:
            i = 1
            for line in dataset.readlines():
                line = line.strip()
                tokens = line.split(' ')
                if len(tokens) <= 2:
                    continue
                if tokens[0] == 'FLASER':
                    msg = LaserScan()
                    num_scans = int(tokens[1])

                    if num_scans != 180 or len(tokens) < num_scans   9:
                        rospy.logwarn("unsupported scan format")
                        continue

                    msg.header.frame_id = 'base_laser_link'
                    t = rospy.Time(float(tokens[(num_scans   8)]))
                    msg.header.stamp = t
                    msg.header.seq = i
                    i  = 1
                    msg.angle_min = -90.0 / 180.0 * pi
                    msg.angle_max = 90.0 / 180.0 * pi
                    msg.angle_increment = pi / num_scans
                    msg.time_increment = 0.2 / 360.0
                    msg.scan_time = 0.2
                    msg.range_min = 0.001
                    msg.range_max = 50.0
                    msg.ranges = [float(r) for r in tokens[2:(num_scans   2)]]

                    bag.write('scan', msg, t)

                    odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans   2):(num_scans   5)]]
                    tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t,'odom','base_link')
                    bag.write('tf', tf_msg, t)

                elif tokens[0] == 'ODOM':
                    odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
                    t = rospy.Time(float(tokens[7]))
                    tf_msg = make_tf_msg(0, 0, 0, t,'base_link','base_laser_link')
                    bag.write('tf', tf_msg, t)

使用说明：

1. 在你slam工作空间下的某个parkage中创建一个script文件夹,与launch文件夹同级目录.
   

2. 把下面代码创建成一个.py文件如convert.py,然后放到script中.

3. 因为他要用到ros库所以必须保存到某个parkage的script文件夹中.

4. cd到这个script文件夹下,然后python convert.py path/ACES.clf path/aces.bag 转化成功.

5. 转化成功后可以cd到你保存bag文件的文件夹中，使用rosbag info，来查看bag包的信息
   
   到这里数据集算是完全处理完成，就可以去跑数据了。

存在的问题

在后续的几个数据集的转化中出现了No handlers could be found for logger "rosout"问题，并且bag并没有转换成功，也使得后面三个数据集无法使用。关于这个错误，由于我的水平也很有限，这个问题也不知道如何解决。如果将这个问题已经解决的大佬或者有后面三个数据集的bag文件的大佬看到了这篇博文，可以在评论区分享一下解决方案或者私聊我也可以，我将不胜感激，跪拜大佬！！

2、误差分析工具使用教程

官方工具下载地址：http://ais.informatik.uni-freiburg.de/slamevaluation/software.php，网站页面也有详细的使用教程，不过是英文的，由于我个人英语水平有限，因此，在工具使用方法的学习上也参考了xuhangxx博主写的《Benchmarking——佛莱堡 SLAM 数据集和误差分析工具》这篇文章。

使用方法：

首先下载Metric Evaluator源码，然后解压到指定的文件夹进行make编译。

正常情况下，第一次编译就会出现如图一样的错误。那么解决方案为：
runtimeError.h文件中加入头文件即可
#include <stdio.h>
#include <stdarg.h>
再次编译，报错就消失了，虽然还有一个warning，但是不影响后续的使用。
到这里，我们已经下载好了我们要使用的工具。接下来就是如何使用这个工具，在使用之前，我们就必须得搞明白工具的输入和输出分别是什么，尤其是我们工具所需要的数据。

输入：

首先官网的描述为：

The two basic inputs are the SLAM logfile, that should contain ROBOTLASER1 or FLASER messages in carmen style and the relations file in the format described below.

从描述中，我们可以看到此工具所需的输入有两个：Relations files和CARMEN logfiles

Relations files

官网描述：

Relations are given in a simple text based format as follows:
# timestamp1 timestamp2 x y z roll pitch yaw
1232658478.161285 1232658644.149293 1.031273 -0.003531 0.000000 0.000000 -0.000000 -0.009755
1232658478.161285 1232658647.058497 2.039538 -0.014451 0.000000 0.000000 -0.000000 -0.016492
Each line describes a relation given by x, y, z, roll, pitch and yaw from timestamp1 to timestamp2. The timestamps have to match those in the logfile that is to be evaluated.

从描述上来看，其实Relations files里面储存的是正确的两个时间戳中位姿变换关系。获取也其实非常简单，细心的同学在第一个图就能看到，在每个数据集中，都有配套的Relations files。

下载方式与数据集相同，同样的全选复制粘贴，不过保存的文档后缀为.relations
到这里我们已经搞定了一个输入了，接下来我们来介绍另一个输入

CARMEN logfiles

首先官网的描述为：

CARMEN style logfiles are text based files, that contain one data element per line. To parse odometry information, we support the standard ODOM format. To parse SLAM outputs we parse the position of either ROBOTLASER1 or the older FLASER messages.
Here is a quick overview of the specific formats:

ODOM x y theta tv rv accel timestamp hostname logger_timestamp

in one line:

ROBOTLASER1 laser_type start_angle field_of_view angular_resolution maximum_range accuracy remission_mode num_readings [range_readings] num_remissions [remission values] laser_pose_x laser_pose_y laser_pose_theta robot_pose_x robot_pose_y robot_pose_theta laser_tv laser_rv forward_safety_dist side_safty_dist turn_axis timestamp hostname logger_timestamp

FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta timestamp hostname logger_timestamp

As we are only concerned about the robot position, the other data,especially laser measurements can be incomplete, i.e, provide 0 range readings if the robot has no range finder.

根据官网描述我们只需要ROBOTLASER1或者FLASER消息即可，那么我们通过上面的描述上，可以看到FLASER消息会更加简洁，因此我们就选择FLASER消息作为输入，我们看一下FLASER消息的主要内容：

FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta timestamp hostname logger_timestamp

看上去所需要的数据挺多，但事实上只需要odom_x、odom_y、odom_theta、timestamp这四个数据，原因我们可以在Metric Evaluator源码找到。具体位置为logutil.cpp中第74行parseFLaser(const std::string& s)

timepos parseFLaser(const std::string& s)
{
  string token;
  stringstream sin(s);
  timepos tp;
  int num_val = 0;

  sin >> token; // skip tag
  // skip ranges
  sin >> num_val;
  for (int i = 0; i < num_val;   i)
    sin >> token;
  // skip laser pose
  for (int i = 0; i < 3;   i)
    sin >> token;
  // read robot pose
  sin >> tp.pos.x >> tp.pos.y >> tp.pos.theta;
  sin >> tp.timestamp;

  return tp;
}

我们从代码中就可以清晰地看到，实际上用到的数据只有odom_x、odom_y、odom_theta、timestamp，因此我们只需要将通过我们SLAM代码矫正后的机器人位姿和当前时刻的时间戳形成FLASER消息即可，则输入形式为：

FLASER 1 0 0 0 0 odom_x odom_y odom_theta timestamp

这里由于[range_readings]为0，因此num_readings取为1，这也可以从上面的代码中看出来，如果num_readings取为0，那么[range_readings]可以不取值，直接跳过即可，如FLASER 0 0 0 0 odom_x odom_y odom_theta timestamp，这个没试过，不过理论上应该没问题。
那么我们已经知道FLASER消息所需的数据，那么如何让将所需数据输出出来形成后缀.log的文件形式呢？
对我个人而言，由于代码水平有限，我只想到了两个解决方案：

1. 直接在工程源码中编写C 读写文件代码，来直接输出出来，但是这样可能会影响代码运行的速度。
2. 由于所需的是.log日志文件，我们可以采用ROS自带日志输出ROS_INFO,这样就可以输出出来日志文件，但是新的问题就是输出出来的文件中含有其他无关的数据，我们需要将多余的数据去掉。其实就很简单了，我们可以设置特殊符号（这里我分别用的@和！）作为标志，在编写C 读写文件代码中，就可以通过判断条件来获取@与！之间的数据了，这个是我写的log文件转FLASER消息的代码，实际上就是简单的C 读写操作，在这里分享给大家顺便我也能赚赚积分。
   
   到这里，我们已经知道了工具所需的输入，下面就简单的介绍一下输出。

输出：

官网描述：

There are two different outputs:
The averaged error, and additional parameters can be written to a file as documentation (-e). Additionally the -eX options enabled to write the errors introduced by individual relations to a file using different sorting options. Unsorted will keep the order as in the relations file for a later matching.

输出的主要是平移和旋转误差，包括欧几里得距离误差和方差。

使用参数：

官网描述：

具体参数都代表什么意思，我就不在描述了，官方介绍的非常详细。下面我就简单的介绍一下使用方法，其实很简单。
cd到metricEvaluator文件夹下，如图
然后输入指令为：

./metricEvaluator -s path/slam.log -r path/aces.relations -o path/aces.log -w "{1.0,1.0,1.0,0.0,0.0,0.0}" -q -e path/error.errors

说明：

1. 这里所有的path/为你所对应文件的位置，一定要替换为实际的路径
2. slam.log代表了输出的FLASER消息文件，名字不一定非得是slam.log
3. aces.relations就是与地图数据集配套的relations关系文件，aces.log为数据集文件，之前为了转换设置为.clf文件，可以在改为.log文件，内容都一样。
4. -w 后面的参数在官网中有具体的解释，"{1.0,1.0,1.0,0.0,0.0,0.0}"得到的为平移误差，"{0.0,0.0,0.0,1.0,1.0,1.0}"得到的为旋转误差
5. -q代表的是输出的为方差，不加为欧几里得误差，官网有说明
6. -e后面跟着的是要输出出来的误差文件，path/为要保存的位置，如果不设置，就默认保存在当前文件夹

论文中的使用：

官网地址：http://ais.informatik.uni-freiburg.de/slamevaluation/index.php

在页面底部有一些相关论文，可以作为参考。

结束语

到这里，所有的内容基本介绍完了，之所以写这篇文章，主要是因为当时在做实验和测试数据集中，踩了很多坑。而且网上对于佛莱堡 SLAM 数据集和误差分析工具使用教程太少或者不太清晰，所以就写了这篇文章。由于我也不怎么写文章，所以这个文章可能有描述不清楚或者错误的地方，可以在评论区或者私聊我指正，感谢大家支持，大家一起加油吧！！

这篇好文章是转载于：学新通技术网