lio livox 保存地图PCD文件

修改 PoseEstimation.cpp 文件如下：

核心是 add by 那一段，添加了保存的代码。

#include "Estimator/Estimator.h"
typedef pcl::PointXYZINormal PointType;

int WINDOWSIZE;
bool LidarIMUInited = false;
boost::shared_ptr> lidarFrameList;
pcl::PointCloud::Ptr laserCloudFullRes;
Estimator* estimator;

ros::Publisher pubLaserOdometry;
ros::Publisher pubLaserOdometryPath;
ros::Publisher pubFullLaserCloud;
tf::StampedTransform laserOdometryTrans;
tf::TransformBroadcaster* tfBroadcaster;
ros::Publisher pubGps;

bool newfullCloud = false;

Eigen::Matrix4d transformAftMapped = Eigen::Matrix4d::Identity();

std::mutex _mutexLidarQueue;
std::queue _lidarMsgQueue;
std::mutex _mutexIMUQueue;
std::queue _imuMsgQueue;
Eigen::Matrix4d exTlb;
Eigen::Matrix3d exRlb, exRbl;
Eigen::Vector3d exPlb, exPbl;
Eigen::Ve