SLAM精度评估—从二维到三维

InfoRich 2022-05-26 发布于重庆

展开全文

作者丨lovely_yoshino

来源丨古月居

简介

最近在群里划水时，看到很多初学的SLAMer面对精度评估这个问题无从下手。而精度评估确实是在SLAM算法实际评估中急需一种手段和工具，本文将从2维室内和3维室外两个层面来向各位展示如何通过各种手段来对SLAM算法完成。

2D评估—基于cartographer

室内的评估方案比较多样，作者也选取了几种方法来实现二维层面上的室内定位性能评估

1.1 直尺和角度计

这种是作为最简单粗糙的一种方式，但是由于其低廉的成本以及边界的衡量性能被广泛适用于一些小厂的开发人员当中。该定位精度需要根据操作人员的操作精度来决定，基本误差在厘米和分米这样的级别。

基本操作步骤：

1.从地图中获取真实场景与地图栅格对应的比例权重、

2. 然后手动发送一个与初始方向保持一致的目标点以及方位，让机器人通过amcl自主导航到该目标位置附近，

3.通过直尺测量真实运动距离与rviz的距离差

4.然后控制机器人原地旋转一定时间，并观察机器人通过角度计测的的误差与rviz上反馈误差的参数

5.以上步骤重复三次以上。这样我们就可以大概的测量得到SLAM算法的定位精度大概处于什么范围了。

1.2 激光测距仪和角度计

这类方法和上文的方法类似，只是将直尺换成了激光测距仪，可以提高人为测量的误差，这类方法也可以较好地评估机器人在直行情况下两个轮子之间的物理误差是否达标。

1.3 动作捕捉仪

动作捕捉仪是作为很多大厂来验证室内定位精度最常用的一种策略方法，其成本也是最高的。

但是其可以对整体的定位实现综合衡量的级别，可以实现类似EVO的结果。根据动捕的资料，动捕的定位精度基本在亚毫米级和度级别。同时动捕可以适用于多机器人场景，在室内场景中完成机器人协同编队的功能。

1.4 软件评价方法

cartographer使用的是图优化的思路，这就导致了cartographer存在回环，而每一次回环其实就是一次精度的校准。基于这一特性，基于这一目的，Cartographer提供了内置工具，可以辅助调参过程或者被用于质量保证目的。

该评估提出的位姿关系度量，cartographer中，我们可以从带有闭环的轨迹中生成ground truth。让带有闭环检测的优化轨迹作为ground truth生成的输入。我们从满足以下条件的闭环检测约束中选择：

min_covered_distance : 闭环之前的最小覆盖距离（以米为单位）被认为自动生成ground truth的候选对象

outlier_threshold_meters :以米为单位的距离，超出该距离被视为异常值

outlier_thresold_meters :以弧度为单位的距离，超出被认为异常值

我们可以假设满足这些要求的相邻轨迹节点的位姿关系在完全优化的轨迹中是局部正确的，尽管从另一个来源的独立输入来看这并不是ground truth ,但是我们现在可以使用它来评估未经闭环优化而生成的局部SLAM结果的质量。

下图说明了该概念，在左侧，ground truth 被可视化为完全优化轨迹的轨迹节点之间的绿色连接，在右侧，红色显示了未优化轨迹中的对应关系。计算出的实际度量是ground truth（绿色）和探针（红色）之间的差。

与繁琐的ground truth设置相比，数据收集过程更加轻松。这种方法的另一个优点是：SLAM系统可以在任何自定义传感器配置中进行评估。

通过以下指令可以获取一系列完全优化的轨迹（.optimized.pbstream文件）ground truth关系

cd <build> # (directory where Cartographer's binaries are located)

./cartographer_autogenerate_ground_truth -pose_graph_filename optimized.pbstream -output_filename relations.pbstream -min_covered_distance 100 -outlier_threshold_meters 0.15 -outlier_threshold_radians 0.02

# 进行评估./cartographer_compute_relations_metrics -relations_filename relations.pbstream -pose_graph_filename test.pbstream

最后结果如下

Abs translational error 0.01944 +/- 0.01819 mSqr translational error 0.00071 +/- 0.00189 m^2Abs rotational error 0.11197 +/- 0.12432 degSqr rotational error 0.02799 +/- 0.07604 deg^2

3D评估—基于LOAM

这类方法基本上就是对应的室外场景，这类场景存在有GPS或者RTK的信息。通常会使用EVO评定工具，这可以拿到绝对轨迹误差(ATE)、相对轨迹误差(RPE)、均方根误差（RMSE）。

2.1 绝对轨迹误差(ATE)

绝对轨迹误差（absolute trajectory error）直接计算相机位姿的真实值与SLAM系统的估计值之间的差。

程序首先根据位姿的时间戳将真实值和估计值进行对齐，然后计算每对位姿之间的差值，并最终以图表的形式输出，该标准非常适合于评估视觉 SLAM 系统的性能。

绝对轨迹误差是估计位姿和真实位姿的直接差值，可以非常直观地反应算法精度和轨迹全局一致性。
估计位姿和groundtruth通常不在同一坐标系中，因此我们需要先将两者对其尺度统一。

2.2 相对轨迹误差(RPE)

相对位姿误差（relative pose error 用于计算相同两个时间戳上的位姿变化量的差，同样，在用时间戳对齐之后，真实位姿和估计位姿均每隔一段相同时间计算位姿的变化量，然后对该变化量做差，以获得相对位姿误差，该标准适合于估计系统的漂移。

相对位姿误差主要描述的是相隔固定时间差$\Delta$两帧位姿差的精度（相比真实位姿），相当于直接测量里程计的误差。

2.3 均方根误差（RMSE）

均方根误差为回归评价指标。衡量观测值与真实值之间的偏差。

2.4 evo 安装

程序安装

pip install evo --upgrade --no-binary evo

# 源码安装#git clone https://github.com/MichaelGrupp/evo#cd evo#pip install --editable . --upgrade --no-binary evo

# 测试evo_ape -h

$ cd evo/test/data$ evo_traj kitti KITTI_00_ORB.txt KITTI_00_SPTAM.txt --ref=KITTI_00_gt.txt -p --plot_mode=xz

详细的步骤可以参考如下链接：

https://blog.csdn.net/qq_39779233/article/details/108299612

测量指标：

evo_ape：绝对位姿误差
evo_rpe：相对位姿误差

常用命令：

evo_traj：绘制轨迹，一条或多条，支持kitti，eurco，tum三种格
evo_res：根据指标比较分析不同SLAM方案轨迹输出结果

不常用命令：

evo_fig：重新打开序列化图的工具
evo_config：用于全局设置和配置文件操作的工具

参数：

-p或–plot: 绘图
-v或–verbose: 输出相关信息（均值，方差等）
-f或–full_check: 检查相关信息（时间戳是否对应，四元数是不是单位四元数）
-a或–align: 对轨迹进行对齐，用ICP的方法，并不是仅仅将起点对齐
–correct_scale: 尺度校正

2.5 数据转换

如果目前没有合适的数据，我们可以把bag包录出来，然后通过如下函数进行转换

odom2tum.msg

float32 xfloat32 yfloat32 zfloat32 q_xfloat32 q_yfloat32 q_zfloat32 q_w

详细代码

#include <ros/ros.h>#include <nav_msgs/Odometry.h>#include 'xxx/odom2tum.h'ros::Publisher pub_gps;ros::Publisher pub_carto;
void callback_gps(const nav_msgs::Odometry::ConstPtr& odom){
    carto_test::odom2tum msg;    msg.x = odom->pose.pose.position.x;    msg.y = odom->pose.pose.position.y;    msg.z = odom->pose.pose.position.z;    msg.q_x = odom->pose.pose.orientation.x;    msg.q_y = odom->pose.pose.orientation.y;    msg.q_z = odom->pose.pose.orientation.z;    msg.q_w = odom->pose.pose.orientation.w;    pub_gps.publish(msg);    ROS_INFO('gps ok');}void callback_carto(const nav_msgs::Odometry::ConstPtr& odom){
    carto_test::odom2tum msg;    msg.x = odom->pose.pose.position.x;    msg.y = odom->pose.pose.position.y;    msg.z = odom->pose.pose.position.z;    msg.q_x = odom->pose.pose.orientation.x;    msg.q_y = odom->pose.pose.orientation.y;    msg.q_z = odom->pose.pose.orientation.z;    msg.q_w = odom->pose.pose.orientation.w;    pub_carto.publish(msg);    ROS_INFO('carto ok');}int main (int argc, char **argv){
    ros::init (argc, argv, 'odom2tum');    ros::NodeHandle n;
    pub_gps = n.advertise<carto_test::odom2tum>('tum_gps', 10, true);    pub_carto = n.advertise<carto_test::odom2tum>('tum_carto', 10, true);    ros::Subscriber sub_gps = n.subscribe<nav_msgs::Odometry>('/odom/by_gps', 10, callback_gps);    ros::Subscriber sub_carto = n.subscribe<nav_msgs::Odometry>('/carto_odom', 10, callback_carto);    ros::Rate loop_rate(50);    while(ros::ok())    {
        ros::spinOnce();        loop_rate.sleep();    }    return 0;}