EVO工具初体验

在同门指导下，使用EVO工具，查看fast-lio和lio-sam运行轨迹对比和RMSE指标。

首先要修改fast-lio和lio-sam src文件下的代码，保存轨迹信息。

fast-lio下/fast-lio/src/fast-lio/src的laserMapping.cpp，发布里程计后面添加如下代码

// 发布里程计
void publish_odometry(const ros::Publisher &pubOdomAftMapped) {
    odomAftMapped.header.frame_id = "camera_init";
    odomAftMapped.child_frame_id = "body";
    odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);  // ros::Time().fromSec(lidar_end_time);
    set_posestamp(odomAftMapped.pose);
    pubOdomAftMapped.publish(odomAftMapped);
    auto P = kf.get_P();
    for (int i = 0; i < 6; i++) {
        int k = i < 3 ? i + 3 : i - 3;
        // 设置协方差 P里面先是旋转后是位置 这个POSE里面先是位置后是旋转 所以对应的协方差要对调一下
        odomAftMapped.pose.covariance[i * 6 + 0] = P(k, 3);
        odomAftMapped.pose.covariance[i * 6 + 1] = P(k, 4);
        odomAftMapped.pose.covariance[i * 6 + 2] = P(k, 5);
        odomAftMapped.pose.covariance[i * 6 + 3] = P(k, 0);
        odomAftMapped.pose.covariance[i * 6 + 4] = P(k, 1);
        odomAftMapped.pose.covariance[i * 6 + 5] = P(k, 2);
    }

	std::ofstream pose1("/home/liuqh/slam_data/fastlio.txt", std::ios::app);
    // pose1.setf(std::ios::scientific, std::ios::floatfield);
    pose1.precision(9);

    static double timeStart =odomAftMapped.header.stamp.toSec();
    auto T1 = ros::Time().fromSec(timeStart);
    ros::Time now_time = odomAftMapped.header.stamp;

    pose1 << now_time<< " "
          << odomAftMapped.pose.pose.position.x << " "
          << odomAftMapped.pose.pose.position.y << " "
          << odomAftMapped.pose.pose.position.z << " "
          << odomAftMapped.pose.pose.orientation.x << " "
          << odomAftMapped.pose.pose.orientation.y << " "
          << odomAftMapped.pose.pose.orientation.z << " "
          << odomAftMapped.pose.pose.orientation.w << std::endl;
    pose1.close();

    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion q;
    transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, odomAftMapped.pose.pose.position.y, odomAftMapped.pose.pose.position.z));
    q.setW(odomAftMapped.pose.pose.orientation.w);
    q.setX(odomAftMapped.pose.pose.orientation.x);
    q.setY(odomAftMapped.pose.pose.orientation.y);
    q.setZ(odomAftMapped.pose.pose.orientation.z);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "body"));  // 发布tf变换
}

下面代码是后面添加的，保存轨迹信息。

	std::ofstream pose1("/home/liuqh/slam_data/fastlio.txt", std::ios::app);
    // pose1.setf(std::ios::scientific, std::ios::floatfield);
    pose1.precision(9);

    static double timeStart =odomAftMapped.header.stamp.toSec();
    auto T1 = ros::Time().fromSec(timeStart);
    ros::Time now_time = odomAftMapped.header.stamp;

    pose1 << now_time<< " "
          << odomAftMapped.pose.pose.position.x << " "
          << odomAftMapped.pose.pose.position.y << " "
          << odomAftMapped.pose.pose.position.z << " "
          << odomAftMapped.pose.pose.orientation.x << " "
          << odomAftMapped.pose.pose.orientation.y << " "
          << odomAftMapped.pose.pose.orientation.z << " "
          << odomAftMapped.pose.pose.orientation.w << std::endl;
    pose1.close();

接下来是lio-sam，在/lio-sam_ws/src/LIO-SAM/src的mapOptmization.cpp中添加下面代码

            laserOdomIncremental.header.stamp = timeLaserInfoStamp;
            laserOdomIncremental.header.frame_id = odometryFrame;
            laserOdomIncremental.child_frame_id = "odom_mapping";
            laserOdomIncremental.pose.pose.position.x = x;
            laserOdomIncremental.pose.pose.position.y = y;
            laserOdomIncremental.pose.pose.position.z = z;
            laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
            if (isDegenerate)
                laserOdomIncremental.pose.covariance[0] = 1;
            else
                laserOdomIncremental.pose.covariance[0] = 0;
        }
        pubLaserOdometryIncremental.publish(laserOdomIncremental);
        // 保存轨迹，path_save是文件目录,txt文件提前建好,/home/xxx/xxx.txt,
        std::ofstream pose1("/home/liuqh/slam_data/liosam.txt", std::ios::app);
        // pose1.setf(std::ios::scientific, std::ios::floatfield);
        pose1.precision(9);
        // if(flag_tum=1){
        static double timeStart = laserOdomIncremental.header.stamp.toSec();
        // flag_tum=0;
        // }
        auto T1 = ros::Time().fromSec(timeStart);
        // tf::Quaternion quat;
        // tf::createQuaternionMsgFromRollPitchYaw(double r, double p, double y);//返回四元数
        pose1 << laserOdomIncremental.header.stamp<< " "
              << laserOdomIncremental.pose.pose.position.x << " "
              << laserOdomIncremental.pose.pose.position.y << " "
              << laserOdomIncremental.pose.pose.position.z << " "
              << laserOdomIncremental.pose.pose.orientation.x << " "
              << laserOdomIncremental.pose.pose.orientation.y << " "
              << laserOdomIncremental.pose.pose.orientation.z << " "
              << laserOdomIncremental.pose.pose.orientation.w << std::endl;
        pose1.close();
    }

其中这些代码是新加入的，保存轨迹信息。

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

重新将fast-lio和lio-sam编译运行，会在指定位置生成如下轨迹信息的txt文件

接着去数据集官网下载轨迹真值，即上图中的street_05.txt，在当前文件夹目录下打开终端

~/slam_data$ evo_ape tum street_03.txt fastlio.txt -a

可以输出fast-lio和轨迹真值的RMSE

加入-p指令后可以可视化对比，在论文中一般就放这些图片。

~/slam_data$ evo_traj tum --ref street_05.txt fastlio.txt -a -p

输入下面命令，可以可视化slam轨迹和真值轨迹的贴合程度。

~/slam_data$ evo_traj tum --ref street_05.txt fastlio.txt -a -p

使用下面指令，可以可视化多条轨迹

~/slam_data$ evo_traj tum --ref street_03.txt liosam.txt fastlio.txt -a -p