A-LOAM代码解读

Comment

由于A-LOAM代码看的时间前前后后跨度时间较长，因此前半部分（scanRegistration、laserOdometry部分）的注释以及理解写的比较详细，而最后一个部分laserMapping的注释以及理解写的比较简略，只大致概括了其中思想，等什么时候有时间再重新写一遍吧。

源码解读A-LOAM

A-LOAM为LOAM的高阶代码实现版本，整体项目框架清晰，主要部分为src文件夹下的三个cpp文件：scanRegistration.cpp，laserOdometry.cpp，laserMapping.cpp

1.scanRegistration.cpp

**主要完成任务：**对每帧点云进行预处理，将每一帧点云即对输入点云进行异常点去除、曲率的计算（计算曲率，将点分成sharp，lessSharp，flat，lessFlat点），以及线束号的计算，提供给后续odometry部分使用

流程框图：
在这里插入图片描述
代码部分：

主函数（main）入口：

int main(int argc, char **argv)
{ros::init(argc, argv, "scanRegistration");   // 设置节点ros::NodeHandle nh;nh.param<int>("scan_line", N_SCANS, 16);  // 16线激光雷达nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);  // 横向分辨率，0.1m，点与点之间距离<0.1m的都被认为是错误检测点printf("scan line number %d \n", N_SCANS);if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64){printf("only support velodyne with 16, 32 or 64 scan line!");return 0;}// 主要的执行函数（对点云的预处理）ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler); // 向odometry部分发布处理完毕的点云pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);if(PUB_EACH_LINE){for(int i = 0; i < N_SCANS; i++){ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);pubEachScan.push_back(tmp);}}ros::spin();  // 程序一直运行return 0;
}

从以上main函数部分可以看到，程序一直在执行（subscribe）函数laserCloudHandler，即对点云的预处理函数，同时在发布（advertise）6类点云，这里主要看laserCloudHandler函数（只看重要部分，不重要，或者简单的部分这里略过）：

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{std::vector<int> scanStartInd(N_SCANS, 0);  // 初始化size=16，值全为0的vector(N_SCANS=16代表16线束)，用于存储每一条scan中，曲率计算的起始位置std::vector<int> scanEndInd(N_SCANS, 0);   // 同上， 这两个vector的作用，可以看下方可视化的图// 将ros格式的输入点云，转换成pcl格式的点云pcl::PointCloud<pcl::PointXYZ> laserCloudIn; pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);std::vector<int> indices;// remove NaN points pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); // 移除点点距离<分辨率的点int cloudSize = laserCloudIn.points.size();// 扫描角度的计算，其实就是yaw角的计算//atan2的范围-pi到+pi，负号是因为激光雷达是顺时针旋转，而角度逆时针才为正float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) +2 * M_PI;    // +2pi，保证endOri >= startOri// 保证一个sweep的扫描角度范围在pi~3pi之内（why？）if (endOri - startOri > 3 * M_PI){endOri -= 2 * M_PI;}else if (endOri - startOri < M_PI){endOri += 2 * M_PI;}bool halfPassed = false;  // 扫描是否过半int count = cloudSize;PointType point;  // XYZI pointstd::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);  // 16线点云，用一个vector进行存储，每个元素代表一个线（scan）的数据for (int i = 0; i < cloudSize; i++){point.x = laserCloudIn.points[i].x;point.y = laserCloudIn.points[i].y;point.z = laserCloudIn.points[i].z;float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;  // 计算pitch angleint scanID = 0;if (N_SCANS == 16){scanID = int((angle + 15) / 2 + 0.5);  // 属于第几根线， +0.5再int，相当于四舍五入操作， 垂直分辨率为2°，范围为-15°~15°// angle=-15°时，scanID=0，angle=15°时，scanID=15if (scanID > (N_SCANS - 1) || scanID < 0)  // 计算得到的scanID不合法{count--;    // count存储合法的点数数量continue;}}else if (N_SCANS == 32){scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}else if (N_SCANS == 64){   if (angle >= -8.83)scanID = int((2 - angle) * 3.0 + 0.5);elsescanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);// use [0 50]  > 50 remove outlies if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0){count--;continue;}}else{printf("wrong scan number\n");ROS_BREAK();}float ori = -atan2(point.y, point.x);if (!halfPassed)   // 判断是否扫描过半{ if (ori < startOri - M_PI / 2){ori += 2 * M_PI;}else if (ori > startOri + M_PI * 3 / 2){ori -= 2 * M_PI;}if (ori - startOri > M_PI)   // 按照理论情况，一个完整scan角度应该正好是2*PI{halfPassed = true;}}else{ori += 2 * M_PI;if (ori < endOri - M_PI * 3 / 2){ori += 2 * M_PI;}else if (ori > endOri + M_PI / 2){ori -= 2 * M_PI;}}float relTime = (ori - startOri) / (endOri - startOri);  // 设ti∈[tk, tk+1], relTime = (ti - tk) / (tk+1 - tk), relTime∈[0, 1]，用作LOAM中的运动补偿point.intensity = scanID + scanPeriod * relTime;  // 用intensity部分，存储每一个点的所属线号（整数部分）+ relTime*scanPeriod（小数部分）laserCloudScans[scanID].push_back(point);  // 每点存储进laserCloudScans中，按照线号进行存储}cloudSize = count;   // 合法的点数数量printf("points size %d \n", cloudSize);pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());  for (int i = 0; i < N_SCANS; i++){ scanStartInd[i] = laserCloud->size() + 5;  // 抛弃每条scan开始的5个点*laserCloud += laserCloudScans[i];scanEndInd[i] = laserCloud->size() - 6; // 抛弃每条scan结束的6个点}// 将存储16线的点云laserCloudScans，集中到一个laserCloud中，同时用两个vector记录，每一线点云的在laserCloud中的起始与结束的index位置for (int i = 5; i < cloudSize - 5; i++) // 对sweep中所有点，使用相邻的前5个点，与后5个点，计算曲率{ float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;  // 曲率计算cloudSortInd[i] = i;  // 点云ID，用作后续曲率排序cloudNeighborPicked[i] = 0;  // 点没被选择过，设置为0.，后续当被选为特征点之后，将被设置为1cloudLabel[i] = 0;   //曲率的分类// Label 2: corner_sharp曲率大（角点）// Label 1: corner_less_sharp, 包含Label 2（曲率稍微小的点，降采样角点）// Label -1: surf_flat（平面点）// Label 0: surf_less_flat， 包含Label -1，因为点太多，最后会降采样}pcl::PointCloud<PointType> cornerPointsSharp;   // sharppcl::PointCloud<PointType> cornerPointsLessSharp;   // lessSharppcl::PointCloud<PointType> surfPointsFlat;       // flatpcl::PointCloud<PointType> surfPointsLessFlat;   // lessFlatfloat t_q_sort = 0;// 将每条scan 6等份，以保证corner点与surface点的均匀分布for (int i = 0; i < N_SCANS; i++){if( scanEndInd[i] - scanStartInd[i] < 6)  // 若scan点数<6，显然不能6等份，直接放弃此scancontinue;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);  // 临时变量，存储lessFlat点for (int j = 0; j < 6; j++)   // 每线（SCAN）点云分成6等份{int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;            // 6等份，闭区间start pointint ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;  // 6等份，闭区间end pointstd::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); // 对6等份内的点云曲率进行升序排列，// 对6等份内的点，按照曲率进行划分，分为sharp，lessSharp，flat，lessFlat四类点int largestPickedNum = 0;for (int k = ep; k >= sp; k--)  // 找每等份内，曲率最大的点，因为排序是升序排列，因此这里倒序搜索{int ind = cloudSortInd[k];   if (cloudNeighborPicked[ind] == 0 &&  // 之前没有被选择过，且曲率大cloudCurvature[ind] > 0.1){largestPickedNum++;if (largestPickedNum <= 2)   // 选2个sharp点{                        cloudLabel[ind] = 2;  // 标记为2类点，即曲率特别大cornerPointsSharp.push_back(laserCloud->points[ind]);  // 存储于cornerPointsSharp中cornerPointsLessSharp.push_back(laserCloud->points[ind]);  // 同时也存储进cornerPointsLessSharp中}else if (largestPickedNum <= 20)  // 选18个lessSharp点{                        cloudLabel[ind] = 1;  // 标记为2类点，即曲率稍大cornerPointsLessSharp.push_back(laserCloud->points[ind]); // 存储进cornerPointsLessSharp中}else{break;}cloudNeighborPicked[ind] = 1;   // 标记此点已被选过for (int l = 1; l <= 5; l++){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) // 查看相邻点距离是否相差过大，如果差异过大说明点云在此不连续，不置位（这里应该就是去除lidar扫描中 ，有遮挡的情况，会导致相邻点深度不连续）{break;}cloudNeighborPicked[ind + l] = 1;  // 被选点的相邻点也被标记}for (int l = -1; l >= -5; l--)  // 同上{float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}cloudNeighborPicked[ind + l] = 1;}}}int smallestPickedNum = 0;for (int k = sp; k <= ep; k++){int ind = cloudSortInd[k];if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < 0.1){cloudLabel[ind] = -1;   // 选4个flat平面点，标记为-1类surfPointsFlat.push_back(laserCloud->points[ind]);smallestPickedNum++;if (smallestPickedNum >= 4) { break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++){ float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}cloudNeighborPicked[ind + l] = 1;}}}for (int k = sp; k <= ep; k++){if (cloudLabel[k] <= 0)  // 将label为-1的flat点，以及其他所有点（label为0）认为是lessFlat点{surfPointsLessFlatScan->push_back(laserCloud->points[k]);}}}// 对lessFlat点做下采样voxel down samplingpcl::PointCloud<PointType> surfPointsLessFlatScanDS;pcl::VoxelGrid<PointType> downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);  downSizeFilter.filter(surfPointsLessFlatScanDS);surfPointsLessFlat += surfPointsLessFlatScanDS;}// 至此，四类点已经分类完毕：// sharp： cornerPointsSharp// sharp + lessSharp：cornerPointsLessSharp// flat：surfPointsFlat// flat + lessFlat：surfPointsLessFlatScanDS（经过了下采样）// 将以上四类点，从pcl格式转成ros格式，并进行发布，用作odometry部分使用// 发布一帧完整sweepsensor_msgs::PointCloud2 laserCloudOutMsg;pcl::toROSMsg(*laserCloud, laserCloudOutMsg);laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;  // 将timestamp从input复制到outputlaserCloudOutMsg.header.frame_id = "/camera_init";pubLaserCloud.publish(laserCloudOutMsg);// 发布完整sweep中的sharp点sensor_msgs::PointCloud2 cornerPointsSharpMsg;pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;cornerPointsSharpMsg.header.frame_id = "/camera_init";pubCornerPointsSharp.publish(cornerPointsSharpMsg);// 发布完整sweep中的sharp+lessSharp点sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;cornerPointsLessSharpMsg.header.frame_id = "/camera_init";pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);// 发布完整sweep中的flat点sensor_msgs::PointCloud2 surfPointsFlat2;pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;surfPointsFlat2.header.frame_id = "/camera_init";pubSurfPointsFlat.publish(surfPointsFlat2);// 发布完整sweep中的flat+lessFlat点sensor_msgs::PointCloud2 surfPointsLessFlat2;pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;surfPointsLessFlat2.header.frame_id = "/camera_init";pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
}

在这里插入图片描述
其中，laserCloudScans、laserCloud、与scanStartInd，scanEndInd之间的关系。

laserOdometry.cpp

**主要完成任务：**基于以上scanRegistration发布的四类点云信息，进行帧与帧之间的点云匹配，得到帧与帧之间的相对位姿变换，得到基于odometry轨迹（由于是帧与帧间匹配得到的位姿，因此会存在累积误差，漂移等问题，是一个粗略的轨迹），后续mapping部分将基于此位姿，进行优化，得到最终累积误差小的位姿输出。

帧间匹配逻辑：
在这里插入图片描述
帧间匹配（sweep-to-sweep）即点云匹配过程，理论上来说，有每一帧的点云，以第一帧的lidar坐标系作为world坐标系，通过不断的pairwise-registration即可得到每一帧相对于world坐标系下的pose，输出一条odometry轨迹，那么整个slam问题就转化成了点云配准（point cloud registration）问题了，但是还需要考虑在实际使用中，需要对lidar进行运动补偿。
运动补偿：

运动补偿的定义，可以参考这篇文章，通俗的来讲，就是lidar在360°旋转的同时，还会有一个前进方向的位移，导致一个lidar扫描周期内，所接收点云其实并不在一个坐标系中，举例如下：

在运动的汽车上,比如说速度为10m/s,直行, 无旋转运动.激光扫描频率为10hz, 也就是一帧0.1秒,雷达在这0.1秒内实现了约360度的旋转.那么0°和360°的激光点, 分别是在时刻0秒和时刻0.1秒扫描的.而第0秒和0.1秒,载具移动了10米/秒*0.1秒=1米.

激光返回的点云中的点, 描述的是激光雷达坐标系下的坐标,假设0秒时,激光雷达扫描得到载具正前方一百米处的一个点A, 记下其在雷达坐标系下的坐标为(0, 100, 0), 扫描完了一圈, 激光雷达输出一帧点云, 时间戳为0.1秒.

也就是说, 激光雷达在0.1秒时, 输出点A的坐标为(100,0,0),而实际, 在0.1秒时, 汽车已经前进了1米, 点A在0.1秒这个时刻激光坐标系的真实坐标应该是(0, 99, 0).

若我们知道每个点扫描的具体时间, 比如知道点B是在0.05秒时被扫描到的，结合这0.05秒载具的相对运动，我们可以对点B的坐标位置进行修复。

因此，如上图所示，lidar在两个扫描周期内，会分别将时间段tk_{tk+1内，以及时间段tk+1}tk+2时间段内扫描得到的点云，打上时间戳tk+1（Pk）,tk+2（Pk+1）进行输出。由于两帧点云都受运动畸变的影响，因此无法直接对Pk与Pk+1做pairwise-registration以获取两帧之间的pose.

因此在对Pk与Pk+1两帧点云做配准之前，需要首先进行运动补偿，目的就是把所有的点云补偿到某一个共同时刻（才能做配准），思路非常直接，基于匀速模型假设，将不同时间戳下的点云，线性投影至统一的时间戳上。举例如下：

假设要将tk~tk+1时间内的所有点云线性投影至tk+1时刻，假设已知tk -> tk+1的 $T_{k+1}^L$ ，则将任意ti∈[tk, tk+1]时刻的点云，投影至tk+1所需的 $T_{(k+1,i)}^L$ 计算如下：
在这里插入图片描述
T是4x4矩阵，无法直接插值，因此在代码中，以上操作分成R与t，t直接线性插值，R使用Eigen::Quaterniond::Identity().slerp函数，即球面线性插值的方式完成。

odometry流程：

假设odometry某次迭代至tk+2时刻，此时输入有：

上次odometry的输出 $T_{k+1}^L$ （即tk -> tk+1的pose）
未经运动补偿的Pk、Pk+1

此次迭代待求的是：

$T_{k+2}^L$ （即tk+1 -> tk+2的pose）

则此次odometry的流程是，对Pk以及Pk+1，利用 $T_{k+1}^L$ （已知）以及 $T_{k+2}^L$ （未知，待求）作运动补偿，均补偿至tk+1时刻，分别得到点云 $\bar P_k$ 以及 $\tilde P_{k+1}$ ，此时两点云已在同一时间戳上，进行点-线、以及点-面匹配，计算残差项，由于残差项是一个关于未知数 $T_{k+2}^L$ 的量，此时利用ceres对残差项进行非线性优化，以求解 $T_{k+2}^L$ ，得到此次odometry的输出。

如此循环往复，得到所有帧间的pose，组合起来，形成一条完整的轨迹。

代码部分：

主要操作都在main函数中完成：

Eigen::Quaterniond q_w_curr(1, 0, 0, 0);  // 待优化变量，即当前帧与上一帧之间的位姿变换
Eigen::Vector3d t_w_curr(0, 0, 0);
int main(int argc, char **argv)
{ros::init(argc, argv, "laserOdometry");ros::NodeHandle nh;nh.param<int>("mapping_skip_frame", skipFrameNum, 2);printf("Mapping %d Hz \n", 10 / skipFrameNum);// 各类点云handler函数，将ros message存入相应的buffer中ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);// odometry节点发布的点云信息，主要是每一帧点云的corner点、surface点、以及odometry部分通过帧间匹配得到的粗略轨迹位姿ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);nav_msgs::Path laserPath;int frameCount = 0;ros::Rate rate(100);  // ros中的一个定时器，与程序末尾的rate.sleep()一起使用以实现控制程序频率的功能，这里设置odometry部分频率为100hzwhile (ros::ok()){ros::spinOnce();  // 触发一次回调函数，即执行一次所有的handler函数，拿到scanRegistration发送端发送过来的点云信息，存储在相应的buffer中if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&  // 确保各buffer中有点云信息!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&!fullPointsBuf.empty()){timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();   // 从各buffer中拿到时间戳timestamp信息（理论上若信息同步，则都是一样的）timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();if (timeCornerPointsSharp != timeLaserCloudFullRes ||      // 若不同步，即时间戳不一样timeCornerPointsLessSharp != timeLaserCloudFullRes ||timeSurfPointsFlat != timeLaserCloudFullRes ||timeSurfPointsLessFlat != timeLaserCloudFullRes){printf("unsync messeage!");ROS_BREAK();}// 将buffer中，ros格式的点云转换成pcl格式点云mBuf.lock();cornerPointsSharp->clear();pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);cornerSharpBuf.pop();cornerPointsLessSharp->clear();pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);cornerLessSharpBuf.pop();surfPointsFlat->clear();pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);surfFlatBuf.pop();surfPointsLessFlat->clear();pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);surfLessFlatBuf.pop();laserCloudFullRes->clear();pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);fullPointsBuf.pop();mBuf.unlock();// initializingif (!systemInited)  // systemInited默认为false, 这里的初始化逻辑是为了当输入是第一帧点云时，以第一帧点云作为World坐标系，因此第一帧点云的pose为Identity，不需要帧间匹配（即下面的else部分），直接跳过下方else部分，输出第一帧odometry pose - > identity(){systemInited = true;std::cout << "Initialization finished \n";}else // 从第二帧开始，做帧间（当前帧与上一帧）匹配// 当前帧的sharp（cornerPointsSharp）与上一帧的sharp + lessSharp（laserCloudCornerLast）做匹配// 当前帧的flat（cornerPointsFlat）与上一帧的flat + lessFlat（laserCloudSurfLast）做匹配{int cornerPointsSharpNum = cornerPointsSharp->points.size();  // 当前帧sharp点数 int surfPointsFlatNum = surfPointsFlat->points.size();   // 当前帧flat点数TicToc t_opt;for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)  // 两帧间的优化求解，循环做2次{corner_correspondence = 0;plane_correspondence = 0;// 定义ceres优化函数ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);ceres::LocalParameterization *q_parameterization =new ceres::EigenQuaternionParameterization();ceres::Problem::Options problem_options;ceres::Problem problem(problem_options);problem.AddParameterBlock(para_q, 4, q_parameterization);  // 将待优化参数传入优化器problem.AddParameterBlock(para_t, 3);pcl::PointXYZI pointSel;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;// 线特征匹配// find correspondence for corner featuresfor (int i = 0; i < cornerPointsSharpNum; ++i)  // 当前帧sharp点，在上一帧的sharp+lessSharp点中寻找correspondence{TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);   // 运动补偿，将当前帧的点投影至扫描周期起点kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);  // 在上一帧sharp+lessSharp点的kdtree中，找最近点int closestPointInd = -1, minPointInd2 = -1;   // int索引变量，用于存储在上一帧找到的最近点、以及次近点的索引if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) // 最近点距离 < 5m{closestPointInd = pointSearchInd[0]; // 最近点索引int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); // 最近点所在线号// 在最近点所在线束的邻近4条线束上找次近点double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;  // search in the direction of increasing scan linefor (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j){// if in the same scan line, continueif (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)continue;// if not in nearby scans, end the loop// NEARBY_SCAN = 2.5, 上方2条线（后续还有下方2条线，因此总共4条线）if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))break;double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *(laserCloudCornerLast->points[j].x - pointSel.x) +(laserCloudCornerLast->points[j].y - pointSel.y) *(laserCloudCornerLast->points[j].y - pointSel.y) +(laserCloudCornerLast->points[j].z - pointSel.z) *(laserCloudCornerLast->points[j].z - pointSel.z);if (pointSqDis < minPointSqDis2){// find nearer pointminPointSqDis2 = pointSqDis;minPointInd2 = j;}}// search in the direction of decreasing scan linefor (int j = closestPointInd - 1; j >= 0; --j){// if in the same scan line, continueif (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)continue;// if not in nearby scans, end the loopif (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))break;double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *(laserCloudCornerLast->points[j].x - pointSel.x) +(laserCloudCornerLast->points[j].y - pointSel.y) *(laserCloudCornerLast->points[j].y - pointSel.y) +(laserCloudCornerLast->points[j].z - pointSel.z) *(laserCloudCornerLast->points[j].z - pointSel.z);if (pointSqDis < minPointSqDis2){// find nearer pointminPointSqDis2 = pointSqDis;minPointInd2 = j;}}}if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid，若最近点以及次近点都已找到，则将当前点、最近点以及次近点的距离加入残差{Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,   // 当前帧的当前点cornerPointsSharp->points[i].y,cornerPointsSharp->points[i].z);Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,  // 上一帧的最近点laserCloudCornerLast->points[closestPointInd].y,laserCloudCornerLast->points[closestPointInd].z);Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,  // 上一帧的次近点laserCloudCornerLast->points[minPointInd2].y,laserCloudCornerLast->points[minPointInd2].z);double s;if (DISTORTION) s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;elses = 1.0;ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);  // 加入残差计算，具体的点到线距离计算在lidarFactor.hpp中problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);corner_correspondence++;  // corner correspondence数目+1}}// 面特征匹配（这一块和上面线特征匹配大同小异）// find correspondence for plane featuresfor (int i = 0; i < surfPointsFlatNum; ++i){TransformToStart(&(surfPointsFlat->points[i]), &pointSel);kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD){closestPointInd = pointSearchInd[0];// get closest point's scan IDint closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;// search in the direction of increasing scan linefor (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j){// if not in nearby scans, end the loopif (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))break;double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *(laserCloudSurfLast->points[j].x - pointSel.x) +(laserCloudSurfLast->points[j].y - pointSel.y) *(laserCloudSurfLast->points[j].y - pointSel.y) +(laserCloudSurfLast->points[j].z - pointSel.z) *(laserCloudSurfLast->points[j].z - pointSel.z);// if in the same or lower scan lineif (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}// if in the higher scan lineelse if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3){minPointSqDis3 = pointSqDis;minPointInd3 = j;}}// search in the direction of decreasing scan linefor (int j = closestPointInd - 1; j >= 0; --j){// if not in nearby scans, end the loopif (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))break;double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *(laserCloudSurfLast->points[j].x - pointSel.x) +(laserCloudSurfLast->points[j].y - pointSel.y) *(laserCloudSurfLast->points[j].y - pointSel.y) +(laserCloudSurfLast->points[j].z - pointSel.z) *(laserCloudSurfLast->points[j].z - pointSel.z);// if in the same or higher scan lineif (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3){// find nearer pointminPointSqDis3 = pointSqDis;minPointInd3 = j;}}if (minPointInd2 >= 0 && minPointInd3 >= 0){Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,surfPointsFlat->points[i].y,surfPointsFlat->points[i].z);Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,laserCloudSurfLast->points[closestPointInd].y,laserCloudSurfLast->points[closestPointInd].z);Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,laserCloudSurfLast->points[minPointInd2].y,laserCloudSurfLast->points[minPointInd2].z);Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,laserCloudSurfLast->points[minPointInd3].y,laserCloudSurfLast->points[minPointInd3].z);double s;if (DISTORTION)s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;elses = 1.0;ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);plane_correspondence++;}}}//printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence);// 至此，点到线以及点到面距离已全部加入残差计算// 若线特征匹配+面特征匹配 匹配对过少if ((corner_correspondence + plane_correspondence) < 10){printf("less correspondence! *************************************************\n");}// 使用ceres优化器进行优化TicToc t_solver;ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.max_num_iterations = 4;options.minimizer_progress_to_stdout = false;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);printf("solver time %f ms \n", t_solver.toc());}printf("optimization twice time %f \n", t_opt.toc());// 更新最新结果// q_w_curr, t_w_curr是当前迭代优化得到的pose（当前帧相对于上一帧）// q_last_curr, t_last_curr是上次迭代的pose（上一帧相对于世界坐标系）// 因此当前帧相对于世界坐标系的pose计算如下：t_w_curr = t_w_curr + q_w_curr * t_last_curr;    q_w_curr = q_w_curr * q_last_curr;}TicToc t_pub;// 发布每一帧的pose（odometry），给后续的mapping部分使用，// publish odometrynav_msgs::Odometry laserOdometry;laserOdometry.header.frame_id = "/camera_init";laserOdometry.child_frame_id = "/laser_odom";laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserOdometry.pose.pose.orientation.x = q_w_curr.x();laserOdometry.pose.pose.orientation.y = q_w_curr.y();laserOdometry.pose.pose.orientation.z = q_w_curr.z();laserOdometry.pose.pose.orientation.w = q_w_curr.w();laserOdometry.pose.pose.position.x = t_w_curr.x();laserOdometry.pose.pose.position.y = t_w_curr.y();laserOdometry.pose.pose.position.z = t_w_curr.z();pubLaserOdometry.publish(laserOdometry);geometry_msgs::PoseStamped laserPose;laserPose.header = laserOdometry.header;laserPose.pose = laserOdometry.pose.pose;laserPath.header.stamp = laserOdometry.header.stamp;laserPath.poses.push_back(laserPose);laserPath.header.frame_id = "/camera_init";pubLaserPath.publish(laserPath);// transform corner features and plane features to the scan end pointif (0)  // KITTI已做运动补偿，因此这里不做{int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();for (int i = 0; i < cornerPointsLessSharpNum; i++){TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);}int surfPointsLessFlatNum = surfPointsLessFlat->points.size();for (int i = 0; i < surfPointsLessFlatNum; i++){TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);}int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++){TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}}// 迭代中的变量更新，在一次迭代的末尾，将当前帧赋值成上一帧pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;laserCloudCornerLastNum = laserCloudCornerLast->points.size();laserCloudSurfLastNum = laserCloudSurfLast->points.size();// std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';// 建立上一帧的kdtreekdtreeCornerLast->setInputCloud(laserCloudCornerLast);kdtreeSurfLast->setInputCloud(laserCloudSurfLast);if (frameCount % skipFrameNum == 0)   // 降频给mapping后端发送点云信息，每5帧发送一次，相当于mapping频率是odometry频率的1/5{frameCount = 0;// 发送一帧的sharp + lessSharp点sensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudCornerLast2.header.frame_id = "/camera";pubLaserCloudCornerLast.publish(laserCloudCornerLast2);// 发送一帧的flat + lessFlat点sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudSurfLast2.header.frame_id = "/camera";pubLaserCloudSurfLast.publish(laserCloudSurfLast2);// 发送一帧的所有点sensor_msgs::PointCloud2 laserCloudFullRes3;pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudFullRes3.header.frame_id = "/camera";pubLaserCloudFullRes.publish(laserCloudFullRes3);}printf("publication time %f ms \n", t_pub.toc());printf("whole laserOdometry time %f ms \n \n", t_whole.toc());if(t_whole.toc() > 100)ROS_WARN("odometry process over 100ms");frameCount++;}rate.sleep();  // 与之前的ros::rate配合，达到控制odometry运行频率的目的}return 0;
}

laserMapping.cpp

**主要完成任务：**基于odometry部分发送的pose位姿信息（高频），以及点云信息（低频），实现基于odometry pose（粗估计）到输出最终pose（精估计）

为什么再laserOdometry中只能做到粗估计，而laserMapping能够做到精估计呢？

原因：laserMapping中使用scan to map的匹配方法，即最新的关键帧scan（绿色线）与其他所有帧组成的全部地图（map）（黑色线）进行匹配，因此laserMapping中的位姿估计方法联系了所有帧的信息，而不是像laserOdometry中仅仅只利用了两个关键帧的信息，所以位姿估计更准确。
在这里插入图片描述
显然，在建图的过程中，将每帧点云与map进行匹配，并将每帧点云加入map，随着轨迹不断拓展，map将会越来越大，这样做效率很低，且不可能用无限大的内存去存储map. 因此LOAM采用栅格地图的方式进行解决。即在程序中，维护一个21x21x11（边长为50m，即1050m x 1050m x 550m）大小的栅格地图，而不是简单的维护一个点云地图。栅格地图即栅格化后的地图，每次将点云按照不同的位置填入不同的栅格中，同时根据当前帧的位置，不断调整栅格地图的位置（相当于一个栅格地图在根据当前帧的位置，不断进行滚动），将当前帧与栅格地图中的localmap进行匹配，以得到最终精估计的位姿。

流程框图：
在这里插入图片描述
主函数（main）入口：

int main(int argc, char **argv)
{ros::init(argc, argv, "laserMapping");ros::NodeHandle nh;float lineRes = 0;   // 分辨率float planeRes = 0;nh.param<float>("mapping_line_resolution", lineRes, 0.4);nh.param<float>("mapping_plane_resolution", planeRes, 0.8);printf("line resolution %f plane resolution %f \n", lineRes, planeRes);downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);   // corner点下采样类downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);  // surface点下采样类// handler函数，将odometry部分发布的点云存入buffer中// 获取odometry中发布的每一帧corner点ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);// 获取odometry中发布的每一帧surface点ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);// 获取odometry中发布的每一帧odometry信息（即pose）ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);// 发布pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);// laserCloudNum = 21 * 21 * 11 = 4851, 即栅格地图的总大小// 栅格地图即一个21 * 21 * 11大小的cubes，这里将corner点与surface点分开存储，分别存储在laserCloudCornerArray与laserCloudSurfArray中for (int i = 0; i < laserCloudNum; i++)  // 释放空间，清空{laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());}// 主线程，运行process函数std::thread mapping_process{process};  ros::spin();return 0;
}

从以上main函数可以看出，mapping输入为每一帧点云的corner、surface点以及odometry pose，再利用mapping对odometry pose进行进一步优化。

void process()
{while(1){while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&!fullResBuf.empty() && !odometryBuf.empty()){mBuf.lock();while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())odometryBuf.pop();// laserOdometry模块对本节点的执行频率进行了控制，laserOdometry模块publish的位姿是10Hz，点云的publish频率则可能没这么高if (odometryBuf.empty())  // 时间戳同步{mBuf.unlock();break;}while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) surfLastBuf.pop();if (surfLastBuf.empty()){mBuf.unlock();break;}while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())fullResBuf.pop();if (fullResBuf.empty()){mBuf.unlock();break;}timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();if (timeLaserCloudCornerLast != timeLaserOdometry ||timeLaserCloudSurfLast != timeLaserOdometry ||timeLaserCloudFullRes != timeLaserOdometry){printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);printf("unsync messeage!");mBuf.unlock();break;}laserCloudCornerLast->clear();pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);cornerLastBuf.pop();laserCloudSurfLast->clear();pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);surfLastBuf.pop();laserCloudFullRes->clear();pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);fullResBuf.pop();q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;   // 里程计部分publish的位姿odometryBuf.pop();  // 考虑到实时性，就把队列里其他的都pop出去，不然可能出现处理延时的情况（这里没看太明白）while(!cornerLastBuf.empty())  // {cornerLastBuf.pop();printf("drop lidar frame in mapping for real time performance \n");  // 为了保证LOAM算法整体的实时性，因Mapping线程耗时>100ms导致的历史缓存都会被清空}mBuf.unlock();TicToc t_whole;transformAssociateToMap();   // 将odometry下的坐标，转换至世界坐标系下// mapping中维护了一个21*21*11的cube（submap，栅格地图），即历史地图，并不断将当前帧与地图进行匹配，以优化odometry输出的粗略的位姿// 首先需要知道当前帧在submap中的何处TicToc t_shift;int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;  // * + 10int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight; // * + 10int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;  // * + 5// 如果此时lidar中心位于（0~25），则center一直位于（10，10，5）// 若lidar中心位于（25~75），则center一直处于（11，11，6）if (t_w_curr.x() + 25.0 < 0)   // int(-2.5) = -2,而期望情况是int(-2.5) = -3，因此手动向左-1centerCubeI--;if (t_w_curr.y() + 25.0 < 0)centerCubeJ--;if (t_w_curr.z() + 25.0 < 0)centerCubeK--;// 调整取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 18, 3 < centerCubeK < 8// 如果cube处于边界，则将cube向中心靠拢一些，方便后续拓展cubewhile (centerCubeI < 3){for (int j = 0; j < laserCloudHeight; j++){for (int k = 0; k < laserCloudDepth; k++){ int i = laserCloudWidth - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; i >= 1; i--){laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeI++;laserCloudCenWidth++;}while (centerCubeI >= laserCloudWidth - 3){ for (int j = 0; j < laserCloudHeight; j++){for (int k = 0; k < laserCloudDepth; k++){int i = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; i < laserCloudWidth - 1; i++){laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeI--;laserCloudCenWidth--;}while (centerCubeJ < 3){for (int i = 0; i < laserCloudWidth; i++){for (int k = 0; k < laserCloudDepth; k++){int j = laserCloudHeight - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; j >= 1; j--){laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeJ++;laserCloudCenHeight++;}while (centerCubeJ >= laserCloudHeight - 3){for (int i = 0; i < laserCloudWidth; i++){for (int k = 0; k < laserCloudDepth; k++){int j = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; j < laserCloudHeight - 1; j++){laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeJ--;laserCloudCenHeight--;}while (centerCubeK < 3){for (int i = 0; i < laserCloudWidth; i++){for (int j = 0; j < laserCloudHeight; j++){int k = laserCloudDepth - 1;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; k >= 1; k--){laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeK++;laserCloudCenDepth++;}while (centerCubeK >= laserCloudDepth - 3){for (int i = 0; i < laserCloudWidth; i++){for (int j = 0; j < laserCloudHeight; j++){int k = 0;pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];for (; k < laserCloudDepth - 1; k++){laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];}laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeCornerPointer;laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =laserCloudCubeSurfPointer;laserCloudCubeCornerPointer->clear();laserCloudCubeSurfPointer->clear();}}centerCubeK--;laserCloudCenDepth--;}int laserCloudValidNum = 0;int laserCloudSurroundNum = 0;for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)   // 5 * 5 * 3 = 75个栅格，每个栅格边长50m，即250m*250m*150m的一个局部地图// 当前帧与以上局部地图进行匹配{for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++){for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++){if (i >= 0 && i < laserCloudWidth &&   // 坐标是否合法j >= 0 && j < laserCloudHeight &&k >= 0 && k < laserCloudDepth){ laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;laserCloudValidNum++;// 构造，要与当前帧匹配的，小submap在大栅格地图中的indiceslaserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;laserCloudSurroundNum++;}}}}laserCloudCornerFromMap->clear();  laserCloudSurfFromMap->clear();for (int i = 0; i < laserCloudValidNum; i++)  // 构建要与当前帧匹配的submap{*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];}int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();// 当前帧与submap进行匹配，对当前帧进行voxel down sampling(submap在构建的时候，也做了voxel down sampling)pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());downSizeFilterCorner.setInputCloud(laserCloudCornerLast);downSizeFilterCorner.filter(*laserCloudCornerStack);int laserCloudCornerStackNum = laserCloudCornerStack->points.size();pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());downSizeFilterSurf.setInputCloud(laserCloudSurfLast);downSizeFilterSurf.filter(*laserCloudSurfStack);int laserCloudSurfStackNum = laserCloudSurfStack->points.size();printf("map prepare time %f ms\n", t_shift.toc());printf("map corner num %d  surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)  // 如果找到一个足够大的局部地图与当前帧匹配{TicToc t_opt;TicToc t_tree;kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);printf("build tree time %f ms \n", t_tree.toc());for (int iterCount = 0; iterCount < 2; iterCount++)    // 整体做两遍ICP{//ceres::LossFunction *loss_function = NULL;ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);ceres::LocalParameterization *q_parameterization =new ceres::EigenQuaternionParameterization();   //这么定义是因为四元数不符合广义的加法ceres::Problem::Options problem_options;ceres::Problem problem(problem_options);problem.AddParameterBlock(parameters, 4, q_parameterization);  //四元数，parameters的前4位problem.AddParameterBlock(parameters + 4, 3);  //从 第parameters + 4 位置开始数3个为平移TicToc t_data;int corner_num = 0;for (int i = 0; i < laserCloudCornerStackNum; i++){pointOri = laserCloudCornerStack->points[i];//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;pointAssociateToMap(&pointOri, &pointSel);kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); if (pointSearchSqDis[4] < 1.0){ std::vector<Eigen::Vector3d> nearCorners;Eigen::Vector3d center(0, 0, 0);for (int j = 0; j < 5; j++){Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,laserCloudCornerFromMap->points[pointSearchInd[j]].y,laserCloudCornerFromMap->points[pointSearchInd[j]].z);center = center + tmp;nearCorners.push_back(tmp);}center = center / 5.0;   // submap中，找到的最近5点的质心Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();for (int j = 0; j < 5; j++){Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;  // 去质心操作covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();  // 计算协方差矩阵（3xN * N*3矩阵可以拆成N个秩一矩阵之和（一列*一行））}Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);// if is indeed line feature// note Eigen library sort eigenvalues in increasing orderEigen::Vector3d unit_direction = saes.eigenvectors().col(2);  // 最大的特征向量Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])  // 最大的特征值比次大的特征值3倍还大，说明submap中找到的5个点在一条直线上，表现为1个大特征值和2个小特征值{ Eigen::Vector3d point_on_line = center;Eigen::Vector3d point_a, point_b;point_a = 0.1 * unit_direction + point_on_line;  // 构建两个虚拟的点，连接成线point_b = -0.1 * unit_direction + point_on_line;ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);// 点到线的距离problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);corner_num++;	}							}/*else if(pointSearchSqDis[4] < 0.01 * sqrtDis){Eigen::Vector3d center(0, 0, 0);for (int j = 0; j < 5; j++){Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,laserCloudCornerFromMap->points[pointSearchInd[j]].y,laserCloudCornerFromMap->points[pointSearchInd[j]].z);center = center + tmp;}center = center / 5.0;	Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);}*/}int surf_num = 0;for (int i = 0; i < laserCloudSurfStackNum; i++){pointOri = laserCloudSurfStack->points[i];//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;pointAssociateToMap(&pointOri, &pointSel);  // 将当前帧的点，转换到世界坐标系（W）下kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);Eigen::Matrix<double, 5, 3> matA0;   // 存储在submap中找到的最近的5个点Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();if (pointSearchSqDis[4] < 1.0){for (int j = 0; j < 5; j++){matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));}// find the norm of planeEigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);double negative_OA_dot_norm = 1 / norm.norm();norm.normalize();// Here n(pa, pb, pc) is unit norm of planebool planeValid = true;for (int j = 0; j < 5; j++){// if OX * n > 0.2, then plane is not fit wellif (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2){planeValid = false;break;}}Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);if (planeValid){ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);surf_num++;}}/*else if(pointSearchSqDis[4] < 0.01 * sqrtDis){Eigen::Vector3d center(0, 0, 0);for (int j = 0; j < 5; j++){Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,laserCloudSurfFromMap->points[pointSearchInd[j]].y,laserCloudSurfFromMap->points[pointSearchInd[j]].z);center = center + tmp;}center = center / 5.0;	Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);}*/}//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);printf("mapping data assosiation time %f ms \n", t_data.toc());TicToc t_solver;ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.max_num_iterations = 4;options.minimizer_progress_to_stdout = false;options.check_gradients = false;options.gradient_check_relative_precision = 1e-4;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);printf("mapping solver time %f ms \n", t_solver.toc());//printf("time %f \n", timeLaserOdometry);//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],//	   parameters[4], parameters[5], parameters[6]);}printf("mapping optimization time %f \n", t_opt.toc());}else{ROS_WARN("time Map corner and surf num are not enough");}transformUpdate();   // 上面的逻辑，用当前帧与局部地图做匹配，得到了一个更为准确的q_w_curr, t_w_curr// 用更为准确的q_w_curr, t_w_curr, 更新增量 q_wmap_wodom, t_wmap_wodom, 即odom坐标系与世界坐标系W之间的转换关系TicToc t_add;   // 栅格地图的构建for (int i = 0; i < laserCloudCornerStackNum; i++){pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel); // L -> Wint cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;  // 找此点在哪一个cubeint cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;if (pointSel.x + 25.0 < 0)cubeI--;if (pointSel.y + 25.0 < 0)cubeJ--;if (pointSel.z + 25.0 < 0)cubeK--;if (cubeI >= 0 && cubeI < laserCloudWidth &&cubeJ >= 0 && cubeJ < laserCloudHeight &&cubeK >= 0 && cubeK < laserCloudDepth){int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;laserCloudCornerArray[cubeInd]->push_back(pointSel); // 存入cube}}for (int i = 0; i < laserCloudSurfStackNum; i++){pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;if (pointSel.x + 25.0 < 0)cubeI--;if (pointSel.y + 25.0 < 0)cubeJ--;if (pointSel.z + 25.0 < 0)cubeK--;if (cubeI >= 0 && cubeI < laserCloudWidth &&cubeJ >= 0 && cubeJ < laserCloudHeight &&cubeK >= 0 && cubeK < laserCloudDepth){int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;laserCloudSurfArray[cubeInd]->push_back(pointSel);}}printf("add points time %f ms\n", t_add.toc());TicToc t_filter;   // 对submap中的点云进行voxel down samplingfor (int i = 0; i < laserCloudValidNum; i++){int ind = laserCloudValidInd[i];pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);downSizeFilterCorner.filter(*tmpCorner);laserCloudCornerArray[ind] = tmpCorner;pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);downSizeFilterSurf.filter(*tmpSurf);laserCloudSurfArray[ind] = tmpSurf;}printf("filter time %f ms \n", t_filter.toc());TicToc t_pub;//publish surround map for every 5 frameif (frameCount % 5 == 0){laserCloudSurround->clear();for (int i = 0; i < laserCloudSurroundNum; i++){int ind = laserCloudSurroundInd[i];*laserCloudSurround += *laserCloudCornerArray[ind];*laserCloudSurround += *laserCloudSurfArray[ind];}sensor_msgs::PointCloud2 laserCloudSurround3;pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);laserCloudSurround3.header.frame_id = "/camera_init";pubLaserCloudSurround.publish(laserCloudSurround3);}if (frameCount % 20 == 0){pcl::PointCloud<PointType> laserCloudMap;for (int i = 0; i < 4851; i++){laserCloudMap += *laserCloudCornerArray[i];laserCloudMap += *laserCloudSurfArray[i];}sensor_msgs::PointCloud2 laserCloudMsg;pcl::toROSMsg(laserCloudMap, laserCloudMsg);laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);laserCloudMsg.header.frame_id = "/camera_init";pubLaserCloudMap.publish(laserCloudMsg);}int laserCloudFullResNum = laserCloudFullRes->points.size();for (int i = 0; i < laserCloudFullResNum; i++){pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);}sensor_msgs::PointCloud2 laserCloudFullRes3;pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);laserCloudFullRes3.header.frame_id = "/camera_init";pubLaserCloudFullRes.publish(laserCloudFullRes3);printf("mapping pub time %f ms \n", t_pub.toc());printf("whole mapping time %f ms +++++\n", t_whole.toc());nav_msgs::Odometry odomAftMapped;odomAftMapped.header.frame_id = "/camera_init";odomAftMapped.child_frame_id = "/aft_mapped";odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);odomAftMapped.pose.pose.orientation.x = q_w_curr.x();odomAftMapped.pose.pose.orientation.y = q_w_curr.y();odomAftMapped.pose.pose.orientation.z = q_w_curr.z();odomAftMapped.pose.pose.orientation.w = q_w_curr.w();odomAftMapped.pose.pose.position.x = t_w_curr.x();odomAftMapped.pose.pose.position.y = t_w_curr.y();odomAftMapped.pose.pose.position.z = t_w_curr.z();pubOdomAftMapped.publish(odomAftMapped);geometry_msgs::PoseStamped laserAfterMappedPose;laserAfterMappedPose.header = odomAftMapped.header;laserAfterMappedPose.pose = odomAftMapped.pose.pose;laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;laserAfterMappedPath.header.frame_id = "/camera_init";laserAfterMappedPath.poses.push_back(laserAfterMappedPose);pubLaserAfterMappedPath.publish(laserAfterMappedPath);static tf::TransformBroadcaster br;tf::Transform transform;tf::Quaternion q;transform.setOrigin(tf::Vector3(t_w_curr(0),t_w_curr(1),t_w_curr(2)));q.setW(q_w_curr.w());q.setX(q_w_curr.x());q.setY(q_w_curr.y());q.setZ(q_w_curr.z());transform.setRotation(q);br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));frameCount++;}std::chrono::milliseconds dura(2);std::this_thread::sleep_for(dura);}
}