修改 PoseEstimation.cpp 文件如下:
核心是 add by 那一段,添加了保存的代码。
#include "Estimator/Estimator.h"
typedef pcl::PointXYZINormal PointType;
int WINDOWSIZE;
bool LidarIMUInited = false;
boost::shared_ptr<std::list<Estimator::LidarFrame>> lidarFrameList;
pcl::PointCloud<PointType>::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<sensor_msgs::PointCloud2Const