使用KITTI里程计开发工具包evaluate_odometry评估ORB_SLAM2运行KITTI单目数据集结果

完成标题任务碰到以下几个问题：

目录

1.ORBSLAM2没有保存单目运行结果的函数，需要修改一下：

2.KITTI里程计开发工具包evaluate_odometry的编译存在问题：mail类没有成员函数finalize()

3.原工具只能评估11到21的序列，可以根据需要评估的序列更改

4.KITTI里程计开发工具包evaluate_odometry的readme文档对使用方法写的不甚清楚，本文对使用方法做出说明

---

1.ORBSLAM2没有保存单目运行结果的函数，需要修改一下：

将System类的成员函数SaveTrajectoryKITTI()里判断传感器的部分直接注释掉：

// if(mSensor==MONOCULAR)
    // {
    //     cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
    //     return;
    // }

之后在相应demo（mono_kitti.cc）里调用保存。

本人由于起初没有看见SaveTrajectoryKITTI()函数，以为ORBSLAM2只支持TUM格式的保存，因此多花了很多时间（以下的（1）（2）两点作为个人日志记录，读者可直接跳过）：

（1）修改ORBSLAM2源码的SaveKeyFrameTrajectoryTUM()函数（此函数对单目结果不适用），在原函数基础上修改使之可以保存单目结果（TUM RGB-D dataset format）。

在system类中添加成员函数void SaveTrajectory(const string &filename)，函数体如下：

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;
}

添加此函数功能之后，就可以根据需要在相应demo里调用

(2)需要将运行结果文件改成KITTI数据集格式

 KITTI数据集格式在开发工具包evaluate_odometry的readme文件里也有说明：

The folder 'poses' contains the ground truth poses (trajectory) for the
first 11 sequences. This information can be used for training/tuning your
method. Each file xx.txt contains a N x 12 table, where N is the number of
frames of this sequence. Row i represents the i'th pose of the left camera
coordinate system (i.e., z pointing forwards) via a 3x4 transformation
matrix. The matrices are stored in row aligned order (the first entries
correspond to the first row), and take a point in the i'th coordinate
system and project it into the first (=0th) coordinate system. Hence, the
translational part (3x1 vector of column 4) corresponds to the pose of the
left camera coordinate system in the i'th frame with respect to the first
(=0th) frame. Your submission results must be provided using the same data

即每帧为一行12列的数据，其实是转换矩阵T的前三行12个元素。但是和每个元素的对应关系说的是“The matrices are stored in row aligned order (the first entries correspond to the first row)”，翻译过来为矩阵行对齐，就只有这样一句简单说明其实也很容易让人困惑，浪费了本人数小时时间。总之每行数据对应矩阵元素如下：

R1 R2 R3 T1 R4 R5 R6 T2 R7 R8 R9 T3

 以上是KITTI数据集格式，而TUM RGBD格式为每帧一行8列：

timestamp t1 t2 t3 q1 q2 q3 q4

（t为平移向量，q为四元数）

以下是将T

UM RGBD格式更改为KITTI数据集格式的python脚本：

import os
import csv
TUM_folder_path = 'results/result_sha/TUMdata/'
KITTI_folder_path = 'results/result_sha/data/'


def read_data(filename):
    col1, col2, col3, col4, col5, col6, col7 = [], [], [], [], [], [], []
    with open(filename, newline='') as csvfile:
        datareader = csv.reader(csvfile, delimiter=' ')
        for row in datareader:
            col1.append(float(row[1]))
            col2.append(float(row[2]))
            col3.append(float(row[3]))
            col4.append(float(row[4]))
            col5.append(float(row[5]))
            col6.append(float(row[6]))
            col7.append(float(row[7]))
    return col1, col2, col3, col4, col5, col6, col7

def write_lists_to_file(filename, *lists):
    with open(filename, 'w') as f:
        n = len(lists[0])  # 假设每个列表的长度相同
        for i in range(n):
            line = ''
            for lst in lists:
                line += str(lst[i]) + '\t'
            line = line[:-1]  # 去掉最后一个制表符
            f.write(line + '\n')


def quaternion2RotateMatrix(q1,q2,q3,qw):
    R1,R2,R3,R4,R5,R6,R7,R8,R9=[],[],[],[],[],[],[],[],[]
    for i in range(len(q1)):
        r1=1-2*(q2[i]*q2[i]+q3[i]*q3[i]); r2=2*(q1[i]*q2[i]-qw[i]*q3[i]); r3=2*(q1[i]*q3[i]+qw[i]*q2[i])
        r4=2*(q1[i]*q2[i]+qw[i]*q3[i]); r5=1-2*(q1[i]*q1[i]+q3[i]*q3[i]); r6=2*(q2[i]*q3[i]-qw[i]*q1[i])
        r7=2*(q1[i]*q3[i]-qw[i]*q2[i]); r8=2*(q2[i]*q3[i]+qw[i]*q1[i]); r9=1-2*(q1[i]*q1[i]+q2[i]*q2[i])
        R1.append(r1)
        R2.append(r2)
        R3.append(r3)
        R4.append(r4)
        R5.append(r5)
        R6.append(r6)
        R7.append(r7)
        R8.append(r8)
        R9.append(r9)
    return R1,R2,R3,R4,R5,R6,R7,R8,R9


for i in range(11):
    TUMfilename = TUM_folder_path + f"{i:02d}.txt" # 构造文件名，使用0填充至两位数
    KITTIfilename = KITTI_folder_path + f"{i:02d}.txt" # 构造文件名，使用0填充至两位数
    if os.path.isfile(TUMfilename): # 检查文件是否存在
         t1, t2, t3, q1, q2, q3, qw = read_data(TUMfilename)
         R1,R2,R3,R4,R5,R6,R7,R8,R9=quaternion2RotateMatrix(q1, q2, q3, qw)
         write_lists_to_file(KITTIfilename, R1,R2,R3,t1,R4,R5,R6,t2,R7,R8,R9,t3)
    else:
        print(f"{TUMfilename} does not exist.")

2.KITTI里程计开发工具包evaluate_odometry的编译存在问题：mail类没有成员函数finalize()

解决此问题有两种办法：

（1）直接将evaluate_odometry.cpp文件里main函数里对应两行注释掉：

if (argc==4) mail->finalize(success,"odometry",result_sha,argv[2]);
  else         mail->finalize(success,"odometry",result_sha);

（2）将finalize的函数体补充在mail.h头文件中

 void finalize (bool success,std::string benchmark,std::string result_sha="",std::string user_sha="")
 {
   if (success)
   {
    msg("Your evaluation results are available at:");
    msg("http://www.cvlibs.net/datasets/kitti/user_submit_check_login.php?benchmark=%s&user=%s&result=%s",benchmark.c_str(),user_sha.c_str(), result_sha.c_str());
   }
   else
   {
    msg("An error occured while processing your results.");
    msg("Please make sure that the data in your zip archive has the right format!");
   }
}

3.原工具只能评估11到21的序列，可以根据需要评估的序列更改

例如本人评估00到10的序列，则更改evaluate_odometry.cpp文件里的bool eval (string result_sha,Mail* mail)函数，将循环改为：

for (int32_t i=0; i<11; i++)
{
...
}

4.KITTI里程计开发工具包evaluate_odometry的readme文档对使用方法写的不甚清楚，本文对使用方法做出说明

本人在网上没有找到其用法的直接说明，起初参考了这篇文档：

GitHub - cristianrubioa/kitti-odometry-eval: KITTI odometry evaluation

感觉没有说清楚，同时多次按其方法尝试都报错了。在查看evaluate_odometry.cpp源码后才弄清楚，使用需要注意可执行文件与参数目录的位置关系,如下：

.
├── data
│   └── odometry
│       └── poses
│           ├── 00.txt
│           ├── 01.txt
│           ├── 02.txt
│           ├── 03.txt
│           ├── 04.txt
│           ├── 05.txt
│           ├── 06.txt
│           ├── 07.txt
│           ├── 08.txt
│           ├── 09.txt
│           └── 10.txt
├── evaluate_odometry
└── results
    └── result_sha
        └── data
            ├── 00.txt
            ├── 01.txt
            ├── 02.txt
            ├── 03.txt
            ├── 04.txt
            ├── 05.txt
            ├── 06.txt
            ├── 07.txt
            ├── 08.txt
            ├── 09.txt
            └── 10.txt

evaluate_odometry可执行文件所在的当前文件夹./下需要有data文件夹、evaluate_odometry、results文件夹， poses文件夹下存放的为ground_truth位姿文件，results/ result_sha/data下则存放的是估计的位姿文件，除了其中的 result_sha文件夹可以自由命名外，其余文件夹、文件及目录关系都须同上。

最后命令行执行：

./evaluate_odometry result_sha

（参数是results文件夹下的子目录，本人孤陋寡闻，这个用法实属没见过。）

note:运行程序后会自动创建一些目录，二次运行则需要删除旧的目录。（擦汗）