LEGO-LOAM轨迹保存与evo评估

si莉亚

已于 2024-05-22 09:49:14 修改

阅读量419

点赞数 9

文章标签： linux 服务器 c++ vscode 自动驾驶

于 2024-05-18 11:33:25 首次发布

本文链接：https://blog.csdn.net/zb0824zb/article/details/139022400

版权

版本：Ubuntu20.04+ros-noetic

续上一篇博客lego-loam运行遇到的问题以及解决办法-CSDN博客

成功运行lego-loam之后，轨迹没有自己保存，所以需要我们手动改一下源码，话不多说直接上干货：

方法1.kitti格式的保存

首先按照kitti-lego-loam源码的readme文件，修改对应的代码，主要内容为：

（1）featureAssociation.cpp

将   float s 10 * (pi->intensity - int(pi->intensity)); 
改为 float s = 1;

// to delete all the code that corrects point cloud distortion
注释掉以下两行
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); 
TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); 


记得把回环检测打开，设置 "loopClosureEnableFlag" 为 "true"

（2）transformfusion.cpp

 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
    {

        currentHeader = laserOdometry->header;

        double roll, pitch, yaw;
        geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
        tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

        transformSum[0] = -pitch;
        transformSum[1] = -yaw;
        transformSum[2] = roll;

        transformSum[3] = laserOdometry->pose.pose.position.x;
        transformSum[4] = laserOdometry->pose.pose.position.y;
        transformSum[5] = laserOdometry->pose.pose.position.z;

        transformAssociateToMap();

        geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                  (transformMapped[2], -transformMapped[0], -transformMapped[1]);

        laserOdometry2.header.stamp = laserOdometry->header.stamp;
        laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
        laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
        laserOdometry2.pose.pose.orientation.z = geoQuat.x;
        laserOdometry2.pose.pose.orientation.w = geoQuat.w;
        laserOdometry2.pose.pose.position.x = transformMapped[3];
        laserOdometry2.pose.pose.position.y = transformMapped[4];
        laserOdometry2.pose.pose.position.z = transformMapped[5];
        pubLaserOdometry2.publish(laserOdometry2);

/added, cout results///

	Eigen::Quaterniond q;

	q.w()=laserOdometry2.pose.pose.orientation.w;
	q.x()=laserOdometry2.pose.pose.orientation.x;
	q.y()=laserOdometry2.pose.pose.orientation.y;
	q.z()=laserOdometry2.pose.pose.orientation.z;

	Eigen::Matrix3d R = q.toRotationMatrix();

	if (init_flag==true)	
	{
		
	H_init<< R.row(0)[0],R.row(0)[1],R.row(0)[2],transformMapped[3],
       	 	 R.row(1)[0],R.row(1)[1],R.row(1)[2],transformMapped[4],
       	 	 R.row(2)[0],R.row(2)[1],R.row(2)[2],transformMapped[5],
          	 0,0,0,1;  
	
	init_flag=false;

	std::cout<<"surf_th : "<<surfThreshold<<endl;

 	}

	H_rot<<	-1,0,0,0,
	    	 0,-1,0,0,
     	   	 0,0,1,0,	
     	    	 0,0,0,1; 
		
	H<<  R.row(0)[0],R.row(0)[1],R.row(0)[2],transformMapped[3],
	     R.row(1)[0],R.row(1)[1],R.row(1)[2],transformMapped[4],
     	     R.row(2)[0],R.row(2)[1],R.row(2)[2],transformMapped[5],
     	     0,0,0,1;  

	

	H = H_rot*H_init.inverse()*H; //to get H12 = H10*H02 , 180 rot according to z axis

	std::ofstream foutC(RESULT_PATH, std::ios::app);

	foutC.setf(std::ios::scientific, std::ios::floatfield);
        foutC.precision(6);
 
	//foutC << R[0] << " "<<transformMapped[3]<<" "<< R.row(1) <<" "<<transformMapped[4] <<" "<<  R.row(2) <<" "<< transformMapped[5] << endl;
	 for (int i = 0; i < 3; ++i)	
	{	 
		for (int j = 0; j < 4; ++j)
        	{
			if(i==2 && j==3)
			{
				foutC <<H.row(i)[j]<< endl ;	
			}
			else
			{
				foutC <<H.row(i)[j]<< " " ;
			}
			
		}
	}

	foutC.close();


//
        laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
        laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
        laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
        tfBroadcaster2.sendTransform(laserOdometryTrans2);

    }

为方便我直接把整个修改后的函数放上来。

然后，在transformfusion.cpp的private中加上以下内容：

    int init_flag=true;

    Eigen::Matrix4f H;
    Eigen::Matrix4f H_init;
    Eigen::Matrix4f H_rot;

    //你的路径
    const string RESULT_PATH = "/media/cairui/Backup Plus/ubuntu20/KITTI/myRes.txt";

记得在对应路径创建你的路径.txt 文件，之后保存编译即可得到kitti格式的轨迹，但是这个保存后的轨迹只能单个拎出来，我输出之后无法进行绝对误差对比，个人感觉是没有时间戳的问题，所以没法和kitti真值比，于是使用了方法2.

方法2.Tum格式的保存

以上的修改甚至可以不用动，直接去mapOptmization.cpp中，修改

void visualizeGlobalMapThread（）这个函数，修改内容参考这位大佬

内容我再列一遍：

//保存轨迹信息（时间戳、xyz、旋转四元数q）
void visualizeGlobalMapThread(){
        ros::Rate rate(0.2);
        while (ros::ok()){
            rate.sleep();
            publishGlobalMap();
        }
        // save final point cloud
        std::cout << "start save final point cloud" << std::endl;
        std::cout << "======================================================" << std::endl;
 
        ofstream f;
        f.open("/home/nb/traj_slam/06_own_tum.txt");						
        f << fixed;
        //std::cout << "traj roll" << cloudKeyPoses6D->points[0].roll << std::endl;
        for(size_t i = 0;i < cloudKeyPoses3D->size();i++)
        {
            float cy = cos((cloudKeyPoses6D->points[i].yaw)*0.5);
            float sy = sin((cloudKeyPoses6D->points[i].yaw)*0.5);
            float cr = cos((cloudKeyPoses6D->points[i].roll)*0.5);
            float sr = sin((cloudKeyPoses6D->points[i].roll)*0.5);
            float cp = cos((cloudKeyPoses6D->points[i].pitch)*0.5);
            float sp = sin((cloudKeyPoses6D->points[i].pitch)*0.5);
 
            float w = cy * cp * cr + sy * sp * sr;
            float x = cy * cp * sr - sy * sp * cr;
            float y = sy * cp * sr + cy * sp * cr;
            float z = sy * cp * cr - cy * sp * sr;
            //save the traj  减掉第0个时间戳。cloudKeyPoses6D->points[0].time == 1317400962.039569
            f << setprecision(6) << (cloudKeyPoses6D->points[i].time-cloudKeyPoses6D->points[0].time) << " " << setprecision(9) << -1*(cloudKeyPoses6D->points[i].x) << " " << -1*(cloudKeyPoses6D->points[i].y) << " " << -1*(cloudKeyPoses6D->points[i].z) << " " << x << " " << y << " " << z << " " << w << endl;
        }
 
        f.close();
}

这位大佬对时间戳进行了修正，所以用这个代码可以完美解决问题，还有一篇用的同样的办法但是没有进行时间戳修正，导致输出的轨迹仍然无法进行对比评估，所以用这个比较好。

之后保存编译即可，记得修改路径。

完~~~~有问题欢迎交流！

2024/5/21补充：

今天在多次运行的时候，发现添加了IMU数据并且开启了回环检测之后，误差居然达到了不开启时的十倍之多，仔细检查发现是开头的几秒内y轴方向发生了极大的漂移，于是觉得是自己的轨迹保存有问题，又换了好几种办法，折腾了半天，最后得出结论，按本文的轨迹保存办法毫无问题，尤其是TUM那个方法，之所以我会出现极大误差，是因为，没有进行IMU和激光雷达的标定，所以使用IMU数据反而会使效果变差。

2024/5/22补充：

至于为什么同样的配置和参数，跑出来有时候效果很好有时候效果很差，我个人的理解是，跟系统运行偶然发生的卡顿（比如你的bag播放时会有某一小段时间因为系统卡顿导致那段bag数据卡没了，或者因为节点卡顿导致的那段接收或者发布有问题）有很大的关系。也是受到这篇博客的启发，发现这个问题也发生在自己身上了，虽然我没用他的那个包，但是原理一样，确实是和系统运行状态有一定的关系，多跑几次，你就会发现效果还是不错。

然后我想到，既然会因为卡顿丢数据导致产生大误差，那怎么才能防止卡顿丢数据呢，除了提高硬件性能，刚刚那位博主提出来压缩bag包，但是我的bag已经很小了，所以没必要，于是我觉得会不会让bag播放的慢一点，会防止卡顿丢太多数据，于是 -r 0.5试了一下，绝了！！！！！果然可以，太合理了哈哈。