SLAM轨迹全局误差计算

SLAM轨迹全局误差计算

一、umeyama算法

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

相似变换的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位姿结果及其他误差标准读者可以从界面获得。