专栏首页科学计算SLAM轨迹全局误差计算

SLAM轨迹全局误差计算

SLAM轨迹全局误差计算

一、umeyama算法

 SLAM结果输出之后,我们需要对其进行测量,判断定位的精确度如何。我们采用如下的高精度optitrack设备对SLAM设备进行GroundTruth数据的采集,结合SLAM的定位数据,我们便可以通过umeyama算法进行两者轨迹间的对齐,然后进一步评估精度误差。

 我们将groundTruth轨迹记作,将slam轨迹记作,我们对这两条符合相似变换的轨迹建立平方根误差:

 首先我们计算均值:

 然后我们再进行方差的计算:

 协方差:

 将其进行SVD分解,记作,并且有:

 相似变换的自由度可以表示为:

 以上就是相似变换的umeyama算法原理,读者有对引理证明感兴趣的,可以阅读Shinji Umeyama的论文《Least-Squares Estimation of Transformation Parameters Between Two Point Patterns》。

 代码如下所示:

inline matrix<3> kabsch(const std::vector<vector<3>> &src, const std::vector<vector<3>> &dst) {
    matrix<3> cov = matrix<3>::Zero();
    for (size_t i = 0; i < src.size(); ++i) {
        cov += src[i] * dst[i].transpose();
    }
    cov = cov * (1.0 / src.size());
    Eigen::JacobiSVD<matrix<3>> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
    const matrix<3> &U = svd.matrixU();
    const matrix<3> &V = svd.matrixV();
    matrix<3> E = matrix<3>::Identity();
    if ((V * U.transpose()).determinant() >= 0.0) {
        E(2, 2) = 1.0;
    } else {
        E(2, 2) = -1.0;
    }

    return V * E * U.transpose();
}

inline std::tuple<double, quaternion, vector<3>> umeyama(std::vector<vector<3>> gt, std::vector<vector<3>> in, bool fix_scale = false) {
    vector<3> gt_avg = vector<3>::Zero();
    vector<3> in_avg = vector<3>::Zero();
    for (size_t i = 0; i < gt.size(); ++i) {
        gt_avg += gt[i];
        in_avg += in[i];
    }
    gt_avg /= (double)gt.size();
    in_avg /= (double)in.size();

    double gt_d2 = 0;
    double in_d2 = 0;
    for (size_t i = 0; i < gt.size(); ++i) {
        gt[i] -= gt_avg;
        in[i] -= in_avg;
        gt_d2 += gt[i].squaredNorm();
        in_d2 += in[i].squaredNorm();
    }

    double S = sqrt(in_d2 / gt_d2);
    if (fix_scale) {
        S = 1;
    }
    matrix<3> R = kabsch(in, gt);
    vector<3> T = S * gt_avg - R * in_avg;

    quaternion q;
    q = R;

    return {S, q, T};
}

二、轨迹对齐结果

 我们使用evo工具直接进行处理,安装之后,我们需要通过如下的命令行进行对齐,并计算rmse误差,这里我们要注意一下,两条轨迹在进行处理之前,必须保证时间已经对齐。读者可以选择硬件时间对齐,或者通过软件进行对齐(Bsplines校准,可参考之前文章)。

evo_ape tum groundTruth.txt slam.txt -va --plot

 使用evo工具进行对齐需要将所有数据按照tum数据集的格式进行txt文件写入。tum数据集格式为[timestamp t.x t.y t.z q.x q.y q.z q.w],-va表示显示详细的计算结果,--plot表示将结果进行图形显示。

 我们将采集到的groundTruth数据与slam数据进行全局轨迹对齐,得到如下的轨迹误差对比图,相对轨迹或者单独轨迹读者可以自行搜索学习:

 灰色轨迹为groundTruth.txt,彩色轨迹为slam.txt,从图中可以看出两条轨迹基本对齐,全局位姿误差(APE)rmse误差大概为5cm左右。umeyama位姿结果及其他误差标准读者可以从界面获得。

SLAM标定系列文章

1. slam标定(三) vio系统

2. slam标定(二) 双目立体视觉

3. slam标定(一) 单目视觉

本文分享自微信公众号 - 科学计算technomania(Quant_Times),作者:大亮

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2021-03-14

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 用于自动驾驶车辆视觉定位的地图存储:ORB-SLAM2的一种拓展

    Persistent Map Saving for Visual Localization for Autonomous Vehicles : An ORB-S...

    用户1150922
  • LiTAMIN:基于正态分布几何近似的SLAM

    声明: 本文只是个人学习记录,侵权可删。论文版权与著作权等全归原作者所有,小王自觉遵守《中华人民共和国著作权法》与《伯尔尼公约》等国内外相关法律,本文禁止转载!...

    3D视觉工坊
  • 论文简述 | 融合关键点和标记的基于图优化的可视化SLAM

    同步定位与建图在移动机器人自主导航中起着重要的作用.大多数视觉SLAM方法使用关键点进行跟踪,但由于光线条件不确定和视点频繁变化,其性能受到任务中不稳定地标的影...

    3D视觉工坊
  • VO视觉里程计

    VO(Visual Odometry)视觉里程计是通过车载摄像头或移动机器人的运动所引起的图像的变化,以逐步估计车辆姿态的过程。

    点云PCL博主
  • OmniSLAM:多鱼眼相机的SLAM系统

    文章:OmniSLAM: Omnidirectional Localization and Dense Mapping for Wide-baseline Mu...

    3D视觉工坊
  • 距离-视觉-惯性里程计:无激励的尺度可观测性(ICRA2021)

    Range-Visual-Inertial Odometry: Scale Observability Without Excitation

    计算机视觉
  • OmniSLAM:多鱼眼相机的SLAM系统

    文章:OmniSLAM: Omnidirectional Localization and Dense Mapping for Wide-baseline Mu...

    点云PCL博主
  • 论文翻译 | 多鱼眼相机的全景SLAM

    提出了一种基于特征的全景图像序列同时定位和建图系统,该系统是在宽基线移动建图系统中从多鱼眼相机平台获得的.首先,所开发的鱼眼镜头校准方法结合了等距投影模型和三角...

    3D视觉工坊
  • 视觉里程计的轨迹评估的工具:evo

    evo是一款用于视觉里程计和slam问题的轨迹评估工具。核心功能是能够绘制相机的轨迹,或评估估计轨迹与真值的误差。支持多种数据集的轨迹格式(TUM、KITTI、...

    3D视觉工坊
  • 视觉里程计的轨迹评估的工具:evo

    evo是一款用于视觉里程计和slam问题的轨迹评估工具。核心功能是能够绘制相机的轨迹,或评估估计轨迹与真值的误差。支持多种数据集的轨迹格式(TUM、KITTI、...

    小白学视觉
  • 经典视觉SLAM框架

    经典的视觉SLAM框架是过去十几年的研究成果。这个框架本身及其所包含的算法已经基本定型,并且已经在许多视觉程序库和机器人程序库中提供。依靠这些算法,我们能够构建...

    博文视点Broadview
  • Kimera实时重建的语义SLAM系统

    Kimera是C++实现的一个具有实时度量的语义SLAM系统,使用的传感器有相机与IMU惯导数据来构建环境语义标注的3D网格,Kimera支持ROS运行在CPU...

    点云PCL博主
  • 从零开始一起学习SALM-ICP原理及应用

    师兄:抱歉,抱歉,最近忙于俗事。我后面一起补上,学习劲头得向你们年轻人学习啊!话说,你最近在研究什么呢?

    用户1150922
  • GPO:快速、准确地进行单眼SLAM初始化的全局平面优化

    初始化对于单目SLAM来说是必须的。本文重点研究了一种基于平面特征的单目SLAM初始化方法。该算法从滑动窗口的单应矩阵估计开始,然后通过全局平面优化(globa...

    用户1150922
  • VDO-SLAM :一种动态目标感知的视觉SLAM系统

    (VDO-SLAM :A Visual Dynamic Object-aware SLAM System)

    用户1150922
  • 计算机视觉方向简介 | 视觉惯性里程计(VIO)

    Visual-Inertial Odometry(VIO)即视觉惯性里程计,有时也叫视觉惯性系统(VINS,visual-inertial system),是融...

    3D视觉工坊
  • vSLAM技术综述

    SLAM是“Simultaneous Localization And Mapping”的缩写,可译为同步定位与建图。概率 SLAM 问题 (the proba...

    SIGAI学习与实践平台
  • vSLAM技术综述

    SLAM是“Simultaneous Localization And Mapping”的缩写,可译为同步定位与建图。概率 SLAM 问题 (the proba...

    小白学视觉
  • 大规模环境下基于语义直方图的多机器人实时全局定位图匹配

    文章:Semantic Histogram Based Graph Matching for Real-Time Multi-Robot Global Loca...

    点云PCL博主

扫码关注云+社区

领取腾讯云代金券