VINS-Fusion 外参标定效果分析（一）

测试流程：

        在 EuRoC V1_01_easy 数据集上测试 VINS-Fusion 在线外参估计效果。使用估计前后不同的外参得到两条完整轨迹，使用 rpg_trajectory_evaluation 工具对比了两条轨迹相对 ground truth 的误差。

rpg_trajectory_evaluation 使用：

#依赖项
python -m pip install numpy matplotlib colorama

#安装
mkdir -p rpg_ws/src
cd rpg_ws/src
git clone https://github.com/uzh-rpg/rpg_trajectory_evaluation.git
git clone https://github.com/catkin/catkin_simple.git
cd ..
catkin_make

#使用
source devel/setup.bash
rosrun rpg_trajectory_evaluation asl_groundtruth_to_pose.py 轨迹保存路径/vio.csv
mv 轨迹保存路径/groundtruth.txt 轨迹保存路径/stamped_traj_estimate.txt
rosrun rpg_trajectory_evaluation analyze_trajectory_single.py 轨迹保存路径

另外，rpg_trajectory_evaluation 默认基于 sim3 对齐，对应纯单目视觉的情况，创建文件可以指定 align_type 和 align_num_frames。

        align_type：

                · sim3：相似变换（用于视觉单目情况）

                · se3：刚体变换（用于视觉双目情况）

                · posyaw：平移加上围绕重力的旋转（用于视觉惯性情况）

                · none：不对齐轨迹

        align_num_frames：

                将在轨迹对齐中使用的位姿数（从头开始）。-1 表示将使用所有位姿。

外参收敛过程：

        下图中 t 表示单位为 m 的平移外参，r 表示单位为 ° 的旋转向量形式的旋转外参。如下所示，外参的旋转部分和平移部分都基本收敛。

        外参的保存方法时在estimator.cpp文件里的optimization函数里，找到“ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));”这行代码，并在这行代码下面添加如下代码：

std::ofstream outfile;
outfile.open(OUTPU_FOLDER+"extrinsic.txt", ios::app);
Eigen::Quaterniond temp_q(para_Ex_pose[0][6], para_Ex_pose[0][3], para_Ex_pose[0][4], para_Ex_pose[0][5]);
Eigen::AngleAxisd temp_rotation_vector(temp_q);
Eigen::Vector3d temp_rotation_v = 57.3 * temp_rotation_vector.angle() * temp_rotation_vector.axis();
outfile << std::to_string(curTime) << " " << para_Ex_pose[0][0] << " " << para_Ex_pose[0][1] << " " << para_Ex_pose[0][2] << " " << temp_rotation_v[0] << " " << temp_rotation_v[1] << " " << temp_rotation_v[2] << std::endl;
outfile.close();

        这样，左目相机与IMU之间的外参就保存在extrinsic.txt文件中，每行数据的顺序是Timestamp、tx、ty、tz、rx、ry、rz。最后使用python编写脚本，利用matplotlib即可画图。

外参估计前后的轨迹误差：

Rotation Error：

使用官方外参

使用估计得到的外参

Translation Error：

使用官方外参

使用估计得到的外参

Scale Error:

使用官方外参

使用估计得到的外参

侧面视角下的轨迹:

使用官方外参

使用估计得到的外参

总结：

现象：

        VIN-Fusion 在线估计得到的外参与官方外参有较大的差别，且使用估计得到的外参可以使轨迹整体精度明显提高。

原因：

        官方外参是在相机内参为 pinhole 模型下标定出来的，与 VIN-Fusion 自己标定出来的 mei 模型的相机内参不匹配，因此重新估计外参可以使精度有明显提升。