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()函数之后。

这样就可以得到无畸变的PCD文件和位姿了。为什么是无畸变的呢？因为feats_undistort是经过IMU补偿之后的点呀，所以保存的PCD是无畸变的鸭。

实际测试的过程中，发现：

1.把保存目录PCD改成PCD1，竟然自动生成了PCD1文件夹。

2.保存之前以为保存会影响计算速度，最后发现不影响计算速度，计算帧率还是9/10帧。

经过这两步，我们就得到了每一帧无畸变的点云和位姿。无缝衔接HBA。