开源MSCKF对比

综述

本次将对比MSCKF里面主要的四种开源的方法，它们按时间顺序分别是msckf_mono,msckf_vio,r-vio,open_vins。主要是对比它们在EuROC数据集上的表现，对比将包括论文对比结果以及实际用数据集测试的结果。实际测试主要对比的是累计误差，初步想法应该是计算初始位置和结束位置之间的距离。

评价指标介绍

参考https://zhuanlan.zhihu.com/p/105428199

• RPE(relative pose error):它描述的是固定时间差的两帧之间位姿差和真实位姿差之间的误差，它的定义如下：
  
  简单推导如下：
  

MSCKF_MONO

MSCKF_VIO

论文中的数据

自己测试
步骤如下：

1. 更改S-MSCKF使之可以保存vio.txt轨迹数据，参考https://blog.csdn.net/slzlincent/article/details/105161113，细微调整如下：
2. 更改都在msckf_vio.cpp中进行，首先在开头的头文件部分加入一个头文件

#include <thread> 

3. 在bool MsckfVio::loadParameters()函数定义结束后面加入如下代码，注意更改 fopen("/home/lincent/s_msckf_ws/vio.txt",“w”);为自己对应的目录

#if 1
static const int p_max_cnt = 10000;
static const int p_cols = 8;
static double *p_log_data = new double[p_max_cnt * p_cols];
static int p_cnt = 0;
void mylog()
{
    FILE* fp = fopen("/home/lincent/s_msckf_ws/vio.txt","w");
    int k = 0;
    while(1)
    {
        if(k < p_cnt)
        {
            for(int i=0; i<p_cols; i++)
            {
                if(i > 0) fprintf(fp, " ");
                fprintf(fp, "%f", p_log_data[p_cols * k + i]);
            }
            fprintf(fp, "\n");
            fflush(fp);
            k++;
        }
        else
        {
            usleep(100000);
        }
    }
    fclose(fp);
}
static std::thread th_log(mylog);
#endif

4. 在const CameraMeasurementConstPtr& msg)函数定义内部的最后部位，return;之前，加入如下代码：

#if 1
    const auto& s = state_server.imu_state;
    const auto& p = s.position;
    const auto& q = s.orientation;
    const auto& v = s.velocity;
    const auto& ba = s.acc_bias;
    const auto& bg = s.gyro_bias;
    double *lp = p_log_data + p_cnt * p_cols;
    lp[0] = s.time;
    lp[1] = p(0);
    lp[2] = p(1);
    lp[3] = p(2);
    lp[4] = q.w();
    lp[5] = q.x();
    lp[6] = q.y();
    lp[7] = q.z();
    //lp[8] = v(0);
    //lp[9] = v(1);
    //lp[10] = v(2);
    //lp[11] = ba(0);
    //lp[12] = ba(1);
    //lp[13] = ba(2);
    //lp[14] = bg(0);
    //lp[15] = bg(1);
    //lp[16] = bg(2);
    //lp[17] = processing_time;
    p_cnt++;
#endif

6. 然后编译运行代码即可，参考https://blog.csdn.net/xiaoxiaoyikesu/article/details/106300423
7. 评测首先要安装evo，参考https://github.com/MichaelGrupp/evo，如果安装后找不到evo，尝试重启电脑
8. 先把euroc的真值转换为tum，记得更改data.csv目录，在euroc的zip包中可以找到

evo_traj euroc data.csv --save_as_tum

9. 正式评测，记得更改相应目录和对应的文件名

#输出轨迹、ape、rpe， p表示plot， va进行轨迹对齐
evo_traj tum vio.txt --ref groundtruth.tum -p -va

evo_ape tum groundtruth.tum vio.txt -p -va

evo_rpe tum groundtruth.tum vio.txt -p -va