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

最新推荐文章于 2024-06-01 22:25:25 发布

3Ｄ视觉工坊

最新推荐文章于 2024-06-01 22:25:25 发布

阅读量1.4k

点赞数 2

文章标签：算法大数据编程语言 python 机器学习

原文链接：https://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247609045&idx=2&sn=b5c9e2b426ff439eaf7c3dde637b5ef0&chksm=fbfd01e1cc8a88f7f84cb099d6acac7bf14e027b9db648a98dbe1382bc864e5f94dacd93584f&scene=126&&sessionid=0

版权

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者丨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 m
Sqr translational error 0.00071 +/- 0.00189 m^2
Abs rotational error 0.11197 +/- 0.12432 deg
Sqr 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 x
float32 y
float32 z
float32 q_x
float32 q_y
float32 q_z
float32 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;
}

然后将转换后的话题tum_gps和tum_gps转换为tum格式的txt

rostopic echo -b tum_gps_carto.bag -p /tum_carto > tum_carto.txt
rostopic echo -b tum_gps_carto.bag -p /tum_gps > tum_gps.txt

参考链接

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

本文仅做学术分享，如有侵权，请联系删文。

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
 4.国内首个面向工业级实战的点云处理课程
 5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
 6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
 7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
 8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

15.国内首个3D缺陷检测教程：理论、源码与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~

3Ｄ视觉工坊

关注

2
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
SLAM精度评估—从二维到三维

点击上方“3D视觉工坊”，选择“星标”干货第一时间送达作者丨lovely_yoshino来源丨古月居简介最近在群里划水时，看到很多初学的SLAMer面对精度评估这个问题无从下手。而精度评估确实是在SLAM算法实际评估中急需一种手段和工具，本文将从2维室内和3维室外两个层面来向各位展示如何通过各种手段来对SLAM算法完成。2D评估—基于cartographer室内的评估方案...
复制链接

扫一扫