fast lio 运行mid360采集的数据，并保存每一帧的点云PCD和位姿

首先我们看到在map_incremental中存在一个保存每一帧PCD文件的代码，因此想利用改代码。

如何修改呢？

一. 改每一帧无畸变点云的PCD的保存代码

    /**************** save map ****************//* 1. make sure you have enough memories/* 2. noted that pcd save will influence the real-time performences **/if (pcd_save_en){int size = feats_undistort->points.size();PointCloudXYZI::Ptr laserCloudWorld( \new PointCloudXYZI(size, 1));for (int i = 0; i < size; i++){// 原来的程序，直接变换到世界坐标系下//RGBpointBodyToWorld(&feats_undistort->points[i], \//                   &laserCloudWorld->points[i]);// 现在改的程序，直接变换到世界坐标系下RGBpointBodyLidarToIMU(&feats_undistort->points[i], \&laserCloudWorld->points[i]);}*pcl_wait_save += *laserCloudWorld;static int scan_wait_num = 0;scan_wait_num ++;if (pcl_wait_save->size() > 0 && pcd_save_interval > 0  && scan_wait_num >= pcd_save_interval){pcd_index ++;string all_points_dir(string(string(ROOT_DIR) + "PCD1/scans_src_") + to_string(pcd_index) + string(".pcd"));pcl::PCDWriter pcd_writer;cout << "current scan saved to /PCD1/" << all_points_dir << endl;pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);pcl_wait_save->clear();scan_wait_num = 0;}}

需要设置的内容为三项，前两项需要在fast-lio/config/mid360.yaml中设置：

1）pcd_save_en 是需要设置为true的；

2）pcd_save_interval需要设置为1（设置为1，表示每一帧保存一次，如果设置为2，就表示每两帧合并成一个pcd文件保存一次），为了和pose对应上，这里就设置为1.

3）由于不需要让点云到IMU世界坐标系下，只需要让点云变换到IMU当前IMU坐标系下，因此：

改为：

            // 原来的程序，直接变换到世界坐标系下//RGBpointBodyToWorld(&feats_undistort->points[i], \//                   &laserCloudWorld->points[i]);// 现在改的程序，直接变换到对应帧的IMU坐标系下RGBpointBodyLidarToIMU(&feats_undistort->points[i], \&laserCloudWorld->points[i]);

RGBpointBodyLidarToIMU(&feats_undistort->points[i], &laserCloudWorld->points[i]);表示通过激光雷达和IMU之间的变换矩阵，把当前激光雷达帧数据变换到当前IMU位姿下。

而RGBpointBodyToWorld(&feats_undistort->points[i], &laserCloudWorld->points[i]);表示首先把当前激光点云变换到当前IMU坐标系下，再变换到IMU世界坐标系下。

因此需要把原来的 RGBpointBodyToWorld 改为 RGBpointBodyLidarToIMU

二.改pose的保存

1.写一个函数

void saveOdometryAndFrame()
{FILE *fp_odometry;string pose_dir = root_dir + "/PCD1/pose_src.txt";fp_odometry = fopen(pose_dir.c_str(),"a");fprintf(fp_odometry, "%d  %lf  %lf  %lf  %lf  %lf  %lf  %lf \n",pcd_index, state_point.pos(0),  state_point.pos(1),state_point.pos(2), geoQuat.w, geoQuat.x,  geoQuat.y,  geoQuat.z);fclose(fp_odometry);}

按照 tx ty tz qw qx qy qz保存数据。

2.把该函数调用写在geoQuat和 state_point赋值之后就可以了，在这里我写到map_incremental()函数之后。