LVI-SAM评估_lvi_sam如何保存轨迹

保存为KITTI格式的轨迹

参考链接：
在Ubuntu20.04系统上LIO-SAM跑KITTI数据集和自己数据集代码修改
A-loam运行kitti及轨迹保存
在LVI-SAM的/src/lidar_odometry/mapOptmization.cpp中找到publishOdometry()函数，在其中添加以下代码（kitti和tum格式二选一即可）：

 // 保存轨迹
         //kitti格式
        // // 位姿输出到txt文档
        std::ofstream pose1("/home/nssc/sbk/code/slam/output/LVI-SAM/pose_kitti.txt", std::ios::app);
        std::ofstream pose2("/home/nssc/sbk/code/slam/output/LVI-SAM/time.txt", std::ios::app);
        pose1.setf(std::ios::scientific, std::ios::floatfield);
        pose2.setf(std::ios::scientific, std::ios::floatfield);
        // pose1.precision(15);

        //save final trajectory in the left camera coordinate system.
        Eigen::Matrix3d rotation_matrix;
        rotation_matrix = Eigen::AngleAxisd(transformTobeMapped[2], Eigen::Vector3d::UnitZ()) * 
                        Eigen::AngleAxisd(transformTobeMapped[1], Eigen::Vector3d::UnitY()) * 
                        Eigen::AngleAxisd(transformTobeMapped[0], Eigen::Vector3d::UnitX());
        Eigen::Matrix<double, 4, 4> mylio_pose;
        mylio_pose.topLeftCorner(3,3) = rotation_matrix;

        mylio_pose(0,3) =  laserOdometryROS.pose.pose.position.x ;
        mylio_pose(1,3) =   laserOdometryROS.pose.pose.position.y;
        mylio_pose(2,3) =   laserOdometryROS.pose.pose.position.z;
        Eigen::Matrix<double, 4, 4> cali_paremeter;
        // cali_paremeter << 4.276802385584e-04, -9.999672484946e-01, -8.084491683471e-03, -1.198459927713e-02,  //00-02
        //                	 -7.210626507497e-03, 8.081198471645e-03, -9.999413164504e-01, -5.403984729748e-02, 
        //                   9.999738645903e-01, 4.859485810390e-04, -7.206933692422e-03, -2.921968648686e-01,
        //                   0,                    0,                   0,                          1;
        cali_paremeter << -1.857739385241e-03,-9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03,     //04-10
                        -6.481465826011e-03, 8.051860151134e-03, -9.999466081774e-01, -7.337429464231e-02,
                        9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01,
                        0,                   0,                   0,                          1;
        /*cali_paremeter << 2.347736981471e-04, -9.999441545438e-01, -1.056347781105e-02, -2.796816941295e-03,    // 03
                            1.044940741659e-02, 1.056535364138e-02, -9.998895741176e-01, -7.510879138296e-02, 
                            9.999453885620e-01, 1.243653783865e-04, 1.045130299567e-02, -2.721327964059e-01,
                            0,                     0,                   0,                          1;*/
                
        Eigen::Matrix<double, 4, 4> myloam_pose_f;
        myloam_pose_f = cali_paremeter * mylio_pose * cali_paremeter.inverse();

        pose1 << myloam_pose_f(0,0) << " " << myloam_pose_f(0,1) << " " << myloam_pose_f(0,2) << " " << myloam_pose_f(0,3) << " "
            << myloam_pose_f(1,0) << " " << myloam_pose_f(1,1) << " " << myloam_pose_f(1,2) << " " << myloam_pose_f(1,3) << " "
            << myloam_pose_f(2,0) << " " << myloam_pose_f(2,1) << " " << myloam_pose_f(2,2) << " " << myloam_pose_f(2,3) << std::endl;

        static auto T1 = timeLaserInfoStamp;
        pose2 << laserOdometryROS.header.stamp - T1  << std::endl;

        pose1.close();
        pose2.close();

        //tum格式
        // 位姿输出到txt文档
        std::ofstream pose0("/home/nssc/sbk/code/slam/output/LVI-SAM/pose_tum.txt", std::ios::app);
        pose0.setf(std::ios::scientific, std::ios::floatfield);
        pose0.precision(15);

        Eigen::Matrix3d temp;
        temp = myloam_pose_f.topLeftCorner(3,3);
        Eigen::Quaterniond quaternion(temp);

        // 获取当前更新的时间 这样与ground turth对比才更准确
   
        pose0 << laserOdometryROS.header.stamp - T1  << " "
            << myloam_pose_f(0,3) << " "
            << myloam_pose_f(1,3) << " "
            << myloam_pose_f(2,3) << " "
            << quaternion.x() << " "
            << quaternion.y() << " "
            << quaternion.z() << " "
            << quaternion.w() << std::endl;

        pose0.close();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

使用KITTI数据集

参考之前的文章：
KITTI数据集转为LVI-SAM使用格式

将LVI-SAM修改为可以使用KITTI的代码

参考链接：LVI-SAM坐标系外参分析与代码修改，以适配各种数据集
代码：github:VI-SAM-Easyused

运行前需要修改的参数

以2011_09_30_0020为例
1.KITTI_lidar.yaml
31行：
T_IMU_LiDAR, LiDAR -> IMU
  extrinsicTranslation: [8.10543972e-01, -3.07054372e-01, 8.02723995e-01]
  extrinsicRotation: [9.999976e-01, -7.854027e-04, 2.024406e-03,
                      7.553071e-04,  9.998898e-01, 1.482454e-02,
                     -2.035826e-03, -1.482298e-02, 9.998881e-01]
修改为 下载解压的raw data
2011_09_30/calib_imu_to_velo.txt中的RT
  extrinsicTranslation: [-8.086759e-01,3.195559e-01,-7.997231e-01]
  extrinsicRotation: [9.999976e-01,7.553071e-04,-2.035826e-03,
                      -7.854027e-04,9.998898e-01,-1.482298e-02,
                      2.024406e-03,1.482454e-02,9.998881e-01]

2.KITTI_camera.yaml
19行：
image_width和image_height
修改为 下载解压的raw data
2011_09_30/2011_09_30_drive_0020_sync/image_00/data/0000000000.png的尺寸大小1226*370

27行：
  fx: 7.215377e+02
  fy: 7.215377e+02
  cx: 6.095593e+02
  cy: 1.728540e+02
修改为  下载解压的raw data
2011_09_30/calib_cam_to_cam.txt中的P_rect_00
P_rect_00: 7.070912e+02 0.000000e+00 6.018873e+02 0.000000e+00 0.000000e+00 7.070912e+02 1.831104e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
P_rect_00 = (fx, 0,cx,delta1,0, fy,cy, delta2,0, 0, 1, delta3)
  fx: 7.070912e+02
  fy: 7.070912e+02
  cx: 6.018873e+02
  cy: 1.831104e+02
  
43行：
extrinsicRotation和extrinsicTranslation
通过kitti_imu2camera0.py计算获得
R:
[0.00781298, -0.0042792,  0.99996033,-0.99985947, -0.01486805,  0.00774856,0.0148343, -0.99988023, -0.00439476]
T:
[1.14389871,-0.31271847,  0.72654605]

3.轨迹
./lidar_odometry/mapOptmization.cpp
保存轨迹部分：
cali_paremeter
修改为
data_odometry_calib/dataset/sequences/06/calib.txt中Tr
 Tr: -1.857739385241e-03 -9.999659513510e-01 -8.039975204516e-03 -4.784029760483e-03 -6.481465826011e-03 8.051860151134e-03 -9.999466081774e-01 -7.337429464231e-02 9.999773098287e-01 -1.805528627661e-03 -6.496203536139e-03 -3.339968064433e-01


cd catkin_ws
catkin_make -j4 -DCATKIN_WHITELIST_PACKAGES=lvi_sam
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

运行

roslaunch lvi_sam KITTI.launch
rosbag play kitti_2011_09_30_drive_0020_synced.bag  
1
2

运行过程遇到的问题

opencv

原因：不同序列的sync中图像的尺寸不同
解决方法：将./config/KITTI_camera.yaml中图像尺寸改为相应运行序列的图像尺寸

image_width: 1226
image_height: 370
1
2

IMU加速度偏置估计出现异常值(部分序列出现)

原因：KITTI_camera.yaml中的转换矩阵参数的问题(解决方法参考链接)+相机和IMU不同步+IMU初始化参数(不同的IMU初始值会导致有些序列error产生或消失)？
论文原因：视觉部分提取特征点数量过少，会导致IMU过大，从而视觉里程计重启
解决方法：无

escalating to SIFGTERM

原因：运行结束，对roslaunch执行ctrl+c后，进程会执行rosspin()后面的程序，在一定时间内程序没有执行完毕，进程会强制退出，并抛出escalating to SIGTERM错误
解决方法：

cd /opt/ros/melodic/lib/python2.7/dist-packages/roslaunch
sudo gedit nodeprocess.py
#修改：_TIMEOUT_SIGINT = 15.0 为_TIMEOUT_SIGINT = 30.0
1
2
3

使用evo进行评估

具体使用方法参考之前的文章：
SLAM精度评估-EVO的使用
这里以2011_09_30_0027序列为例(对应真值07)展示结果
绘制轨迹

#tum:
evo_traj tum pose_tum.txt --ref=07_gt_tum.txt -p --plot_mode=xyz
1
2

轨迹评估
绝对姿态误差

mkdir results
evo_ape tum 07_gt_tum.txt pose_tum.txt -r full -va --plot --plot_mode xz --save_results results/07_ape.zip
1
2

相对姿态误差

#每10米比较一次相对真实轨迹的轨迹误差：
 evo_rpe tum pose_tum.txt 07_gt_tum.txt -r trans_part --delta 10  --delta_unit m -v -as --plot --plot_mode xz --save_results results/07_10m_rpe.zip
1
2

#每10°比较一次相对真实轨迹的轨迹误差：
 evo_rpe tum 07_gt_tum.txt pose_tum.txt -r angle_deg --delta 10  --delta_unit d -v -as --plot --plot_mode xz --save_results results/07_10°_rpe.zip
1
2

评估出现的问题

生成轨迹长度或pose数量与对应真值不同

原因：生成的kitti格式没有时间戳进行对齐
解决方法：github evo提供了contrib/kitti_poses_and_timestamps_to_trajectory.py，将kitti真值的times.txt和pose转为tum格式(项目里保存的轨迹格式也应为tum格式)
github evo链接

python kitti_poses_and_timestamps_to_trajectory.py ./kitti_gt/data_odometry_poses/dataset/poses/07.txt ./kitti_gt/data_odometry_calib/dataset/sequences/07/times.txt 07_gt_tum.txt
1