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