EVO获取SLAM与真值对齐后的轨迹

EVO获取SLAM与真值对齐后的轨迹

Motivation

有时候我们需要对SLAM跑出来的轨迹于真值进行对齐，虽然evo工具能够画出对齐后的轨迹，但是我们还是希望能够得到对齐的轨迹原始数据，自己作图，更好的控制。

步骤：

第一步：通过源码安装evo

注意一定是源码安装，后续需要在源码上更改保存对齐后的轨迹。

下载

git clone https://github.com/MichaelGrupp/evo.git

安装

pip install --editable .

安装网上已经有很多博客了，遇到问题的话多找找。

第二步：源码更改（以evo_ape为例）

evo_ape主要是进入main_ape.py文件，找到对齐后的轨迹变量，将对齐的轨迹保存到目标路径中。

找到main_ape.py的ape()函数，找到下面这段对齐代码：

# Align the trajectories.
    only_scale = correct_scale and not align
    alignment_transformation = None
    if align or correct_scale:
        logger.debug(SEP)
        alignment_transformation = lie_algebra.sim3(
            *traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align))
    elif align_origin:
        logger.debug(SEP)
        alignment_transformation = traj_est.align_origin(traj_ref)

在后面把traj_est（对齐后的轨迹）的信息保存下来：

print("write traj")
np.savetxt('your_store_path/filename.txt', np.c_[traj_est.positions_xyz],
               fmt='%f', delimiter=' ')

当然，这只是最简单的保存了对齐后的xyz数据，保存的方法也是直接在源码中更改，你也可以采用更灵活的方法保存数据，这里只是告诉如何找到保存后的数据。至于如何保存看个人习惯。

第三步：重新编译源码

进入evo文件夹：

pip install --editable .

提示：

Installing collected packages: evo
  Attempting uninstall: evo
    Found existing installation: evo 1.22.0
    Uninstalling evo-1.22.0:
      Successfully uninstalled evo-1.22.0
  Running setup.py develop for evo
Successfully installed evo

第四步：运行evo_ape命令

evo_ape tum groundtruth.txt slam.txt -p -vas

就可以在你第二步源码中设置的路径下找到slam与真值对齐后的轨迹。