前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >SLAM轨迹全局误差计算

SLAM轨迹全局误差计算

作者头像
猫叔Rex
发布2021-03-16 15:39:30
1.6K0
发布2021-03-16 15:39:30
举报
文章被收录于专栏:科学计算

SLAM轨迹全局误差计算

一、umeyama算法

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

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

 首先我们计算均值:

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

 协方差:

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

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

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

 代码如下所示:

代码语言:javascript
复制
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校准,可参考之前文章)。

代码语言:javascript
复制
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标定(一) 单目视觉

本文参与 腾讯云自媒体同步曝光计划,分享自微信公众号。
原始发表:2021-03-14,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 傅里叶的猫 微信公众号,前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体同步曝光计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • SLAM轨迹全局误差计算
    • 一、umeyama算法
      • 二、轨迹对齐结果
      领券
      问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档