使用 evo 工具评测 VI ORB SLAM2 在 EuRoC 上的结果

2017年11月2日

screen-shot-2017-11-02-at-16-56-19
最近把 ORB SLAM2 作者融合 IMU 版本的算法《Visual-Inertial Monocular SLAM with Map Reuse》论文读了一遍，作者没有开放源代码，所以我是使用王京实现的一版开源代码（https://github.com/jingpang/LearnVIORB）进行了一些实验。LearnVIORB 代码有一些崩溃的 bug，在 Mac 上面编译有问题，不过实现了作者文中的基本思想，是非常好的一份实现。

由于只有 EuRoC 测试集有 VIO 的数据，我使用了 EuRoC 测试集进行测试，不过在实际运行中遇到了几个问题。

0、系统环境
macOS Sierra v10.12.6

1、evo 安装
以下 1）或 2）任选一种方式即可：

1）使用 pip3 安装
执行如下命令：

pip3 install evo --upgrade

1	pip3 install evo --upgrade

2）使用源代码安装：
执行如下命令：

git clone https://github.com/MichaelGrupp/evo.git
cd evo
pip install . --upgrade

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

cd evo

pip install . --upgrade

2、修改 System 保存 TUM 观测值
由于 ORB SLAM2 并没有提供单目相机或者 VIO 保存 EuRoC 形式数据的方式，这里我们仍然使用 SaveTrajectoryTUM 方法保存观测值。
不过需要注意的是，由于 SaveTrajectoryTUM 不允许保存单目相机数据，我们修改一下创建一个新的函数 SaveTrajectory 用来保存单目 VIO 数据。

1）在 System.h 中添加函数头：

    void SaveTrajectory(const string &filename);

1	void SaveTrajectory(const string &filename);

2）在 System.cc 中添加函数体：

    void System::SaveTrajectory(const string &filename)
    {
        cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
        
        vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
        sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
        
        // Transform all keyframes so that the first keyframe is at the origin.
        // After a loop closure the first keyframe might not be at the origin.
        cv::Mat Two = vpKFs[0]->GetPoseInverse();
        
        ofstream f;
        f.open(filename.c_str());
        f << fixed;
        
        // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
        // We need to get first the keyframe pose and then concatenate the relative transformation.
        // Frames not localized (tracking failure) are not saved.
        
        // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
        // which is true when tracking failed (lbL).
        list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
        list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
        list<bool>::iterator lbL = mpTracker->mlbLost.begin();
        for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
            lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
        {
            if(*lbL)
                continue;
            
            KeyFrame* pKF = *lRit;
            
            cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
            
            // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
            while(pKF->isBad())
            {
                Trw = Trw*pKF->mTcp;
                pKF = pKF->GetParent();
            }
            
            Trw = Trw*pKF->GetPose()*Two;
            
            cv::Mat Tcw = (*lit)*Trw;
            cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
            cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
            
            vector<float> q = Converter::toQuaternion(Rwc);
            
            f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
        }
        f.close();
        cout << endl << "trajectory saved!" << endl;
    }

void System::SaveTrajectory(const string &filename)

{

cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;

vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();

sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);

// Transform all keyframes so that the first keyframe is at the origin.

// After a loop closure the first keyframe might not be at the origin.

cv::Mat Two = vpKFs[0]->GetPoseInverse();

ofstream f;

f.open(filename.c_str());

f << fixed;

// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).

// We need to get first the keyframe pose and then concatenate the relative transformation.

// Frames not localized (tracking failure) are not saved.

// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag

// which is true when tracking failed (lbL).

list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();

list<double>::iterator lT = mpTracker->mlFrameTimes.begin();

list<bool>::iterator lbL = mpTracker->mlbLost.begin();

for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),

lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)

{

if(*lbL)

continue;

KeyFrame* pKF = *lRit;

cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);

// If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.

while(pKF->isBad())

{

Trw = Trw*pKF->mTcp;

pKF = pKF->GetParent();

}

Trw = Trw*pKF->GetPose()*Two;

cv::Mat Tcw = (*lit)*Trw;

cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();

cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);

vector<float> q = Converter::toQuaternion(Rwc);

f << setprecision(6) << *lT << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;

}

f.close();

cout << endl << "trajectory saved!" << endl;

}

3、修改 VIO 测试代码 EuRoC 时间戳 （可选）
在我们运行时由于 EuRoC 时间戳超过了 int64 的位数，所以如果你不是 ROS 环境而是使用自己写的 main 函数来读取 imu 时间戳会有问题。
这里面我的处理方式是将时间戳的前五位舍弃，保留后面的内容，然后再乘以 $1e-9$ 转换成以s为单位的数据。相关代码参考：

        comma = line.find(',',0);
        string temp = line.substr(0,comma);
        line.substr(TIMESTAMP_OFFSET,comma - TIMESTAMP_OFFSET) << endl;
        imuTimeStamp = strtoll(line.substr(TIMESTAMP_OFFSET,comma - TIMESTAMP_OFFSET).c_str(), NULL, 10);

comma = line.find(',',0);

string temp = line.substr(0,comma);

line.substr(TIMESTAMP_OFFSET,comma - TIMESTAMP_OFFSET) << endl;

imuTimeStamp = strtoll(line.substr(TIMESTAMP_OFFSET,comma - TIMESTAMP_OFFSET).c_str(), NULL, 10);

以及：

        ORB_SLAM2::IMUData tempImu(grad[0],grad[1],grad[2],acc[0],acc[1],acc[2],imuTimeStamp*1e-9);

1	ORB_SLAM2::IMUData tempImu(grad[0],grad[1],grad[2],acc[0],acc[1],acc[2],imuTimeStamp*1e-9);

最后在 VIO 运行完毕后使用这一函数保存数据到 Data/EuRoC/CameraTrajectory.txt。

4、修改 EuRoC 真值的时间戳（可选）
如果你像我一样为了防止数据溢出范围做了步骤3中的操作，则你同样需要对 EuRoC 的真值进行相应的修改，我这里提供了一个简单的代码进行这一操作。
下载地址：convert_euroc_csv-py

代码如下：

import sys
import numpy
import argparse
import csv

TIMESTAMP_OFFSET = 5

def convert_csv(src_path, dst_path):
    # 读取csv至字典
    csv_file = open(src_path, "r")
    reader = csv.reader(csv_file)

    w_csv_file = open(dst_path, "w")
    writer = csv.writer(w_csv_file)

    # 建立空字典
    # result = ""

    for item in reader:
        # 忽略第一行
        if reader.line_num == 1:
            writer.writerow(item)
            continue
        # result += item[0] + " " + item[1] + "\n"

        # original: timestamp qx qy qz qwtx ty tz
        # tum : timestamp tx ty tz qx qy qz qw

        # 保持和 mono_vio_euroc 里面处理一致
        timestamp = str(int(item[0][TIMESTAMP_OFFSET:]))
        item[0] = timestamp

        # print(item)

        writer.writerow(item)

        # result += timestamp + " " + item[1] + " " + item[2] + " " + item[3] + " " + item[5] + " " + item[6] + " " + item[7] + " " + item[4] + "\n"

    csv_file.close()
    w_csv_file.close()
    # print(result)

    # tum_file = open(tum_list, "w")
    # tum_file.write(result)
    # tum_file.close()

if __name__=="__main__":
    # parse command line
    parser = argparse.ArgumentParser(description='''
    This script tries to convert the time stamp to our demo format. 
    ''')
    parser.add_argument('first_file', help='data.csv in state_groundtruth_estimate0/')
    parser.add_argument('second_file', help='file where you want to save the converted csv')

    args = parser.parse_args()

    first_file = args.first_file
    second_file = args.second_file

    convert_csv(first_file, second_file)

import sys

import numpy

import argparse

import csv

TIMESTAMP_OFFSET = 5

def convert_csv(src_path, dst_path):

# 读取csv至字典

csv_file = open(src_path, "r")

reader = csv.reader(csv_file)

w_csv_file = open(dst_path, "w")

writer = csv.writer(w_csv_file)

# 建立空字典

# result = ""

for item in reader:

# 忽略第一行

if reader.line_num == 1:

writer.writerow(item)

continue

# result += item[0] + " " + item[1] + "\n"

# original: timestamp qx qy qz qwtx ty tz

# tum : timestamp tx ty tz qx qy qz qw

# 保持和 mono_vio_euroc 里面处理一致

timestamp = str(int(item[0][TIMESTAMP_OFFSET:]))

item[0] = timestamp

# print(item)

writer.writerow(item)

# result += timestamp + " " + item[1] + " " + item[2] + " " + item[3] + " " + item[5] + " " + item[6] + " " + item[7] + " " + item[4] + "\n"

csv_file.close()

w_csv_file.close()

# print(result)

# tum_file = open(tum_list, "w")

# tum_file.write(result)

# tum_file.close()

if __name__=="__main__":

# parse command line

parser = argparse.ArgumentParser(description='''

This script tries to convert the time stamp to our demo format.

''')

parser.add_argument('first_file', help='data.csv in state_groundtruth_estimate0/')

parser.add_argument('second_file', help='file where you want to save the converted csv')

args = parser.parse_args()

first_file = args.first_file

second_file = args.second_file

convert_csv(first_file, second_file)

然后只要运行这一转换脚本即可：

tools/evaluate/convert_euroc_csv.py Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data.csv Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data2.csv

1	tools/evaluate/convert_euroc_csv.py Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data.csv Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data2.csv

5、使用 evo 进行评估
按照步骤3 中操作，我们得到了运行的结果 Data/EuRoC/CameraTrajectory.txt，按照步骤4 中操作，我们得到了转换时间戳后的真值 Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data2.csv。

使用以下命令来运行 evo_ape 评估 Average Pose Error：

evo_ape euroc Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data2.csv Data/EuRoC/CameraTrajectory.txt  -va --plot --save_results results/ORB.zip

1	evo_ape euroc Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data2.csv Data/EuRoC/CameraTrajectory.txt -va --plot --save_results results/ORB.zip

其中 evo_ape 是评估工具，euroc 表示后面输入参数是 euroc 的真值格式。我们输入的就是类似 TUM 的形式即可。
更详细的使用说明可以参见 evo 的官方说明：
https://github.com/MichaelGrupp/evo

运行后得到如下结果：
screen-shot-2017-11-02-at-16-44-28

并且生成了如下两幅图：
wechatimg431
wechatimg432

可以看出我们的 VIO 曲线拟合基本正确，但是最大误差大约在 0.3m 左右，平均误差 0.14m 左右。这一误差还是不小的，与作者论文中的结论还有一些差距，应该还需要一些细致的优化工作。

可能有同学会疑惑，EuRoC 数据集不是坐标系为 IMU/Body 坐标系嘛？为什么我们没有输入 $T_{CB}$ 的标定参数呢？原来，在 evo 框架中，并不关心标定问题，它使用了 Umeyama 算法对两个轨迹做拟合，用拟合的结果来进行评估。

好了，evo 评估 VI ORB SLAM 的真值方法就到这里，总体感觉这一工具还是非常实用的，对于做 VSLAM 研究的人来说会有很大帮助。

常见问题：
1、错误：You are using pip version 9.0.3, however version 10.0.1 is available.
如果在安装 evo 时遇到如下问题：
You are using pip version 9.0.3, however version 10.0.1 is available.
并且无法使用 pip（比如网络差）升级命令：

pip install –upgrade pip

1	pip install –upgrade pip

或者：

python -m pip install –upgrade pip

1	python -m pip install –upgrade pip

进行升级。请尝试以下步骤

1）在官网下载 pip：
https://pypi.org/project/pip/#files

下载其中的 tar.gz 包，或者下载 pip-10-0-1-tar

2）解压后进入 pip 所在目录运行：

sudo python setup.py install

1	sudo python setup.py install

参考文献：
https://michaelgrupp.github.io/evo/

About The Author

skylook

增强现实、图像识别技术爱好者。

9 Comments

kingkong
这个好像很不错啊，感谢大佬
2018年4月16日回复
匿名
感谢大佬，请问VINS运行保存的.csv和真值怎么对比啊
2018年5月10日回复

xiaoshui
你好请问你解决这个问题了吗?
2018年12月11日回复

孟庆瑜
你好，请问步骤3中，修改时间戳的代码应该放在程序的哪里才能使用呢？小白求教，十分感谢！
2018年6月2日回复

匿名
不用那么复杂
用evo的就行
from evo.tools import file_interface
traj = file_interface.read_tum_trajectory_file(first_file)
traj.timestamps = traj.timestamps / 1e9
file_interface.write_tum_trajectory_file(second_file, traj)
2018年6月22日回复

夏天
请问这个语句如何使用?
2018年7月4日回复

匿名
你好，请问你解决了吗
2019年1月22日回复

greenjoker
大佬你好，我在ros上跑orb-slam程序，请问该怎么保存下轨迹信息以便用evo来评估？
2019年1月16日回复
匿名
请问std是什么指标呀？
2019年3月27日回复

技术刘

使用 evo 工具评测 VI ORB SLAM2 在 EuRoC 上的结果

About The Author

skylook

Add a Comment

Related Posts

About The Author

skylook

Add a Comment