ROS评估LIO-SAM算法教程（2）

本节将延续第一节内容来详细说明evo工具如何来评估slam算法。

http://t.csdnimg.cn/aFgYS

1.evo工具下载

1.1.pip直接安装最新的稳定发行版

pip install evo --upgrade --no-binary evo

1.2.源码安装

从github上下载evo工具

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

在evo路径下执行以下命令

pip install -i https://pypi.tuna.tsinghua.edu.cn/simple evo --upgrade --no-binary evo

需要准备的拓展工具--合并文件的python程序

kitti_poses_and_timestamps_to_trajectory.py

下载地址：https://github.com/MichaelGrupp/evo/blob/master/contrib/kitti_poses_and_timestamps_to_trajectory.py

2.LIO-SAM代码相关修改

由于需要评估slam算法的性能，需要算法在跑完数据集后打印出路径结果，并且与真值进行比对，因此需要在算法内部增加一些代码来生成pose.txt文件来储存轨迹。

以下代码部分参考使用KITTI跑LIOSAM并完成EVO评价_ros bag转kitti-CSDN博客这篇文章

加入ring和time
            int rowIdn = -1;
            if (has_ring == true){
                rowIdn = laserCloudIn->points[i].ring;
            }
            else{
                float verticalAngle, horizonAngle;
                verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
            }

            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;

            if (rowIdn % downsampleRate != 0)
                continue;


            int columnIdn = -1;
            if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
            {
                float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
                static float ang_res_x = 360.0/float(Horizon_SCAN);
                columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
                if (columnIdn >= Horizon_SCAN)
                    columnIdn -= Horizon_SCAN;
            }
            else if (sensor == SensorType::LIVOX)
            {
                columnIdn = columnIdnCountVec[rowIdn];
                columnIdnCountVec[rowIdn] += 1;
            }
            
            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
                continue;

            if (has_ring == true)
                thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
            else {
                    float ori = -atan2(thisPoint.y, thisPoint.x);
                    if (!halfPassed) {
                        if (ori < cloudInfo.startOrientation - M_PI / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > cloudInfo.startOrientation + M_PI * 3 / 2) {
                            ori -= 2 * M_PI;
                        }
                        if (ori - cloudInfo.startOrientation > M_PI) {
                            halfPassed = true;
                        }
                    } else {
                        ori += 2 * M_PI;
                        if (ori < cloudInfo.endOrientation - M_PI * 3 / 2) {
                            ori += 2 * M_PI;
                        } else if (ori > cloudInfo.endOrientation + M_PI / 2) {
                            ori -= 2 * M_PI;
                        }
                    }
                    float relTime = (ori - cloudInfo.startOrientation) / cloudInfo.orientationDiff;
                    // 激光雷达10Hz，周期0.1
                    laserCloudIn->points[i].time = 0.1 * relTime;
                    thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
            }

// 位姿输出到txt文档
std::ofstream pose2("zz/pose2.txt", std::ios::app);
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(pose_in.yaw, Eigen::Vector3d::UnitZ()) * 
                  Eigen::AngleAxisd(pose_in.pitch, Eigen::Vector3d::UnitY()) * 
                  Eigen::AngleAxisd(pose_in.roll, Eigen::Vector3d::UnitX());
Eigen::Matrix<double, 4, 4> mylio_pose;
mylio_pose.topLeftCorner(3,3) = rotation_matrix;

mylio_pose(0,3) = pose_in.x;
mylio_pose(1,3) = pose_in.y;
mylio_pose(2,3) = pose_in.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,
                    -6.481465826011e-03, 8.051860151134e-03, -9.999466081774e-01, -7.337429464231e-02,
                     9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01,     //04-10
                     0                    0,                   0,                          1;*/
cali_paremeter << 2.347736981471e-04, -9.999441545438e-01, -1.056347781105e-02, -2.796816941295e-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();

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

pose2.close();
————————————————

                            版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
                        
原文链接：https://blog.csdn.net/weixin_47552638/article/details/129759500

打印轨迹保存
          	    //保存轨迹，path_save是文件目录,txt文件提前建好,/home/xxx/xxx.txt,
   			std::ofstream pose1("/home/lsj/pose1.txt", std::ios::app);
			pose1.setf(std::ios::scientific, std::ios::floatfield);
			pose1.precision(9);
            // if(flag_tum=1){
			static double timeStart = laserOdometryROS.header.stamp.toSec();
            // flag_tum=0;
            // }
			auto T1 =ros::Time().fromSec(timeStart) ;
            // tf::Quaternion quat;
            // tf::createQuaternionMsgFromRollPitchYaw(double r, double p, double y);//返回四元数
			pose1<< laserOdometryROS.header.stamp -T1<< " "
              << -laserOdometryROS.pose.pose.position.x << " "
              <<- laserOdometryROS.pose.pose.position.z << " "
              << -laserOdometryROS.pose.pose.position.y<< " "
              << laserOdometryROS.pose.pose.orientation.x << " "
              << laserOdometryROS.pose.pose.orientation.y << " "
              << laserOdometryROS.pose.pose.orientation.z << " "
              << laserOdometryROS.pose.pose.orientation.w << std::endl;
			pose1.close();

这里的pose1.txt和pose2.txt分别保存的是tum格式的轨迹和位姿，同时这两个文件夹需要提前建好，保存文本的路径可以同步进行修改。

3.真值数据

3.1准备文件

这两个压缩包在该网址下。【kitti】ground_truth 数据包官网下载链接

 合并时间戳文件和pose文件（以07序列为参考）

3.2提取时间戳文件

该文件为 "data_odometry_calib\dataset\sequences\07\times.txt"

3.3 提取pose文件

 该文件为"data_odometry_poses\dataset\poses\07.txt"

3.4 合并文件的python程序

kitti_poses_and_timestamps_to_trajectory.py

3.5 输出tum格式文件

将上述3个文件放在同一目录下，并执行如下命令。("07_gt_tum.txt"为输出的txt文件，执行前先新建一个空白的txt)

python kitti_poses_and_timestamps_to_trajectory.py  00.txt times.txt  00_gt_tum.txt

到此就完成了slam算法跑出的轨迹数据和轨迹真值数据，之后的工作就是根据自身需求来使用evo进行评估