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 point
std::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 angle
int 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=15
if (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);
else
scanID = 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.，后续当被选为特征点之后，将被设置为1
cloudLabel[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; // sharp
pcl::PointCloud<PointType> cornerPointsLessSharp; // lessSharp
pcl::PointCloud<PointType> surfPointsFlat; // flat
pcl::PointCloud<PointType> surfPointsLessFlat; // lessFlat
float 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等份，直接放弃此scan
continue;
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 point
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; // 6等份，闭区间end point
std::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 sampling
pcl::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部分使用
// 发布一帧完整sweep
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; // 将timestamp从input复制到output
laserCloudOutMsg.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在两个扫描周期内，会分别将时间段tktk+1内，以及时间段tk+1tk+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 T_{k+1}^L Tk+1L，则将任意ti∈[tk, tk+1]时刻的点云，投影至tk+1所需的 T ( k + 1 , i ) L T_{(k+1,i)}^L T(k+1,i)L计算如下：
T是4x4矩阵，无法直接插值，因此在代码中，以上操作分成R与t，t直接线性插值，R使用Eigen::Quaterniond::Identity().slerp函数，即球面线性插值的方式完成。
odometry流程：
假设odometry某次迭代至tk+2时刻，此时输入有：
- 上次odometry的输出 T k + 1 L T_{k+1}^L Tk+1L（即tk -> tk+1的pose）
- 未经运动补偿的Pk、Pk+1
此次迭代待求的是：
- T k + 2 L T_{k+2}^L Tk+2L（即tk+1 -> tk+2的pose）
则此次odometry的流程是，对Pk以及Pk+1，利用 T k + 1 L T_{k+1}^L Tk+1L（已知）以及 T k + 2 L T_{k+2}^L Tk+2L（未知，待求）作运动补偿，均补偿至tk+1时刻，分别得到点云 P ˉ k \bar P_k Pˉk以及 P ~ k + 1 \tilde P_{k+1} P~k+1 ，此时两点云已在同一时间戳上，进行点-线、以及点-面匹配，计算残差项，由于残差项是一个关于未知数 T k + 2 L T_{k+2}^L Tk+2L的量，此时利用ceres对残差项进行非线性优化，以求解 T k + 2 L T_{k+2}^L Tk+2L，得到此次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部分频率为100hz
while (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();
// initializing
if (!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 features
for (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 line
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
// if in the same scan line, continue
if (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 point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
// search in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// if not in nearby scans, end the loop
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 point
minPointSqDis2 = 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;
else
s = 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 features
for (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 ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// if not in nearby scans, end the loop
if (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 line
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// if in the higher scan line
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// search in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if not in nearby scans, end the loop
if (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 line
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
// find nearer point
minPointSqDis3 = 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;
else
s = 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 odometry
nav_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 point
if (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';
// 建立上一帧的kdtree
kdtreeCornerLast->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; // * + 10
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight; // * + 10
int 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，因此手动向左-1
centerCubeI--;
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向中心靠拢一些，方便后续拓展cube
while (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在大栅格地图中的indices
laserCloudSurroundInd[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 order
Eigen::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 plane
Eigen::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 plane
bool planeValid = true;
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well
if (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 -> W
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; // 找此点在哪一个cube
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;
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 sampling
for (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 frame
if (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);
}
}