【LOAM系列】一：LOAM论文及代码阅读笔记
LOAM
2014 Ji Zhang
LOAM中文注释版：https://github.com/cuitaixiang/LOAM_NOTEDLOAM
笔记及A-LOAM源码阅读：https://www.cnblogs.com/wellp/p/8877990.htmlLOAM
代码解析：https://blog.csdn.net/liuyanpeng12333/article/details/82737181ALOAM
试跑及程序注释：[https://blog.csdn.net/unlimitedai/article/details/105711240
LOAM论文和程序代码的解读https://blog.csdn.net/robinvista/article/details/104379087
LOAM原理+代码: https://zhuanlan.zhihu.com/p/111388877
紧耦合LIO：https://zhuanlan.zhihu.com/p/567254168
1、论文
LOAM: Lidar Odometry and Mapping in Real-time
1.1 整体框架
LOAM主要包含两个模块，一个是Lidar Odometry，即使用激光雷达做里程计计算两次扫描之间的位姿变换；另一个是Lidar Mapping，利用多次扫描的结果构建地图，细化位姿轨迹。由于Mapping部分计算量较大，所以计算频率较低（1Hz），由Mapping校准细化Odometry过程中计算出来的轨迹。
**传感器:**一次完整的三维扫描为sweep，雷达平面的一次扫描为scan，世界坐标系{W}是与初始位置的{L}重合
雷达扫描：
实现3维雷达；一次完整的三维扫描为sweep，雷达平面的一次扫描为scan；
将点云分为两类：边线点(edge point角点)和平面点(planar point)，分别对应采取点到线及点到面的约束；在实际的测试中，平面点占总点云的80%。
位姿变化估计采用高频粗估计+低频优化。两套算法来实现定位。(laser odometry & mapping odometry)
其中，laser odometry高频粗估计是在雷达坐标系下，通过点到线及点到面距离，利用LM得到雷达位姿估计；
mapping odometry低频位姿精估计，是在世界坐标系下，通过点到线，点到面的距离，得到世界坐标系下的雷达位姿估计优化。
低频位姿精估计是1HZ，与sweep频率相同。也就是一次完整的sweep会进行一次完整的位姿估计；
使用线性运动模型进行激光雷达畸变矫正，假设激光雷达的角速度和线速度随时间平滑且连续，
1.2 特征提取
Pk 第k次整体sweep中得到的点云。X^L_{(k,i)} 表示第k个sweep中的i点在雷达坐标系下的坐标。
设i ∈ Pk，表示点i是第k次整体sweep中的点。选取i点在同一个scan中相邻的前后的5个点X ( k , j ) ^L , j ∈ S , j ≠ i ，
曲率计算公式
C = Σ j ( X ( k , i ) L − X ( k , j ) L ) / ( ∣ S ∣ ⋅ ∣ ∣ X ( k , i ) L ∣ ∣ ) C=\Sigma _j(X^L_{(k,i)}-X^L_{(k,j)})/(|S|·||X^L_{(k,i)}||) C=Σj(X(k,i)L−X(k,j)L)/(∣S∣⋅∣∣X(k,i)L∣∣)
注意，论文中使用的是平面雷达，每个scan扫描区域是半平面，即空间的平面，雷达一次scan会得到空间平面与雷达扫描平面交线上的点，会得到一条线的点，但是，如果雷达扫过角点，则就是类似平面v字型的点。
即：如果是平面，前后的5个点与i的距离相加=0，
如果是边线角点，则c > 0
c的计算方程的分母，用于归一化c值，防止，因为雷达的距离而带来的c值得过大或小。
基于c值对扫描中的点进行排序，然后选择具有最大c的特征点，即边缘点；以及最小c，即平面点。
均匀分布特征点，将一次扫描线分为四个相同的子区域。每个子区域最多可提取2个边缘点和4个平面点(全是平面点或边缘点也可以)。仅当点i的c值大于或小于阈值且周围的点没有被选择且所选点的数量不超过最大个数时，点i才可以被选为边或平面点。
避免选择包围点的点，或局部平面上与激光束大致平行的点，避免位于被遮挡区域边界上的点
1.3 特征匹配
里程测量算法估计激光雷达在sweep内的运动
从Pk+1中提取边缘点和平面点i，将Pk+1帧的点投影到sweep的开始，将Pk投影到K帧下的结束，到与之对应的点和线，
边缘特征点的匹配： 对于i点，在Pk中查找最近邻点j，然后在Pk中相邻扫描线中查找j最近邻点k，j k 形成一条线，计算点i到这条线的距离，ij向量与il向量叉乘的模除以jl向量的模
平面特征点的匹配：对于点i，在Pk中找到最近邻点j，然后在j同一扫描线上找到最近的一个平面点k，再在相邻扫描线上找到最近平面点l，j k l 三点构成一个面。计算点i到面的距离
1.4 帧间运动估计
位姿差值：
激光雷达运动在扫描期间以恒定的角速度和线速度建模，不同时间接收到的点在sweep扫描内进行线性插值姿势变换
T^L(k+1)为[tk+1, t]的位姿变换
T k + 1 L = [ t x , t y , t z , θ x , θ y , θ z ] T T^L_{k+1 }=[t_x , t_y , t_z , θ_x , θ_y , θ_z ]^T Tk+1L=[tx,ty,tz,θx,θy,θz]T
最小化距离d,LM求解点投影的位姿
计算P k 是使用TransformToEnd()将P k投影本k时刻sweep的结束，即k+1时刻sweep的开始来得到的。
而当前时刻k+1时刻的点云映射到k+1时刻的初始是使用TransformToStart()实现。
=======================================================================================================
1）输入是已经消除畸变（运动补偿，就是把Pk点云中的所有的点都映射到同一个时刻tk+1，消除由于车辆运动和lidar的旋转导致的点云畸变现象）的上一次扫描的点云
、此次扫描过程中逐渐被观测到的点云Pk+1、[tk+1, t]时刻的pose之间的增量 
（pose之间的transform)；
2）如果是一次新的扫描就让
等于0；
3）然后从点云Pk+1中提取边点和平面点作为特征点，分别构成边特征点集合
和面特征点集合
；
4）对于集合中的每一个边点以及平面点我们找到在
的对应边和对应平面，然后根据距离公式，构建点到线和点到面的距离残差方程，进而估计Lidar的运动，其中优化变量是
，优化目标是残差距离，即求一个最优的
，使得点到线以及点到面的距离最小；
5）每个残差方程都分配一个权重，特征点到对应边（或者对应面）的距离越大就分配越小的权重，如果距离大于一定阈值，就把权重设置成0；
6）利用LM法（源码中用的是高斯牛顿法）对这个非线性最小二乘问题进行求解，得到
，这样就可以更新 
；
7）当非线性优化收敛或者迭代次数到达预设值后，break；
8）如果本次odometry算法已经到达一次扫描的结束时刻就把Pk+1的所有点映射到tk+2时刻得到
，完成对Pk+1点云的运动补偿，否则就只输出已经更新的
，为下一次的odometry算法做准备。
======================================================================================================
1.5 建图
(低频) 一次完整sweep执行一次
将k+1次雷达扫描到通过Odometry得到的位姿投影到地图坐标系中，得到了未经校准的地图，然后再使用特征匹配优化位姿
在Mapping时，LOAM维护了一个以当前位置为中心，具有多个block的子地图（10m的立方体）。当Lidar采集到的点落入到这个子地图的某些block时，将从这个子地图提取出对应的block，然后在这些block中像特征点提取那样，提取当前扫描中角点和平面点近邻的对应点，但有不同之处，并不像第2小节那样提取2个近邻角点确定直线或者3个平面点确定平面，而是提取更多的点进行拟合，因为子地图是多次扫描构成的结果。对于当前扫描的一个角点，提取对应子地图中多个近邻角点，通过奇异值分解求出这些角点的主方向，从而的到直线方程，从而计算点到直线距离；对于平面点，通过寻找最小特征值对应的向量得到拟合平面的法向量，从而计算点到平面距离。再进行优化，得到对齐扫描与子地图的位姿纠正参数，之后将原有的轨迹进行纠正，便得到了细化的轨迹。
mapping算法就是将已经消除畸变的
点云先转换到全局坐标系变成
，然后与局部地图（local map或者称为submap，源码中使用的是三维栅格cube做的局部地图管理）做特征匹配，将配准好的点云
添加到
中，
一旦超过预设的范围，就删除范围外的点云，源码中submap的范围是250m * 250m * 150m，需要注意的是，源码中对submap的管理并不是基于时序的滑动窗口方式，而是空间范围划分方式，这就是为什么最后的frame对应的submap会包含早期的frames。
A-LOAM中选取最近邻5个点 ，设平面方程为ax + by + cz + 1 = 0，求解平面法向量X=(a,b,c)，将最近邻5个点带入平面方程，得到A X = B AX=BAX=B的方程，其中A为有5个点云构成的5*3的矩阵，B = [ − 1 , − 1 , − 1 , − 1 , − 1 ] T ，使用QR分解得到平面法向量X=(a,b,c)。并对向量进行归一化得到(a’, b’, c’)。
点(x 0 , y 0 , z 0 x_0,y_0,z_0 )到平面的计算公式： a b s ( a x 0 + b y 0 + c z 0 + 1 ) / ( a 2 + b 2 + c 2 ) a b s ( a_{ x 0 }+ b _{y 0 }+ c _{z 0} + 1 ) / \sqrt( a_ 2 + b_ 2 + c_ 2 ) abs(ax0+by0+cz0+1)/(a2+b2+c2)
体素滤波均匀化点
通过回环来统计偏移量
2、A-LOAM代码
2.1 scanRegistration.cpp
scanRegistration.cpp用来读取激光雷达数据并且对激光雷达数据进行整理，去除无效点云以及激光雷达周围的点云，并且计算每一个点的曲率，按照提前设定好的曲率范围来分离出不同类型的点。
removeClosedPointCloud剔除距离小于阈值的点
//双指针法剔除距离过小的点
for (size_t i = 0; i < cloud_in.points.size(); ++i)
{
if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
continue;
cloud_out.points[j] = cloud_in.points[i];
j++;
}
laserCloudHandler点云回调函数
// todo: 接收原始点云数据的回调函数，点云剔除，转为velodyne数据类型，计算点云曲率，特征点提取
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
剔除点云，pclNaN剔除，和距离阈值剔除
点云转为平行扫描线
1）为每个点云点找到它所对应的扫描线（SCAN）
计算点的俯仰角，计算垂直视场角，决定这个激光点所在的scanID， 通过计算垂直视场角确定激光点在哪个扫描线上（N_SCANS线激光雷达，±15°的垂直视场，垂直角度分辨率2°，-15°时的scanID = 0，落在视场外的点不要了
2）**确定某个点云所在扫描线的角度范围，**要计算每个点的时间戳，首先需要确定这点云的角度范围，使用中的atan2( )函数计算点云点的水平角度，atan2(double y，double x)函数计算的角度如图所示
计算点云角度范围，目的是求解点所在的扫描线，并通过论文中方法，对齐时间戳，根据时间占比求出所有点对应的角度范围。其中根据扫描范围在不同象限做了不同处理，考虑了扫描范围在二三象限时候，对角度进行加2pi将其约束到 -pi/2 - 3/2 * pi范围内。类似地如果扫描范围在一四象限，需要将结束点角度也减2pi。
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;
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
3）按照点云在原始扫描线里的角度分配时间戳
//relTime 是一个0~1之间的小数，代表占用扫描时间的比例，乘以扫描时间得到真实扫描时刻，scanPeriod扫描时间默认为0.1s
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;//赋值给点云的强度，整数部分是scanID，小数部分代表该激光点距sweep开始时刻的时间差
laserCloudScans[scanID].push_back(point);
点云曲率计算
前五个点，后五个点的距离差作为曲率，ALOAM里面面的点云是有序的，直接使用相邻的5个点
A-LOAM里面将一条scan划分为6段，LOAM论文里面是划分成4段
特征点选取
次边缘点是前20个是包含了前两个边缘点的，次边缘点是在上一帧的KD-tree里面使用的，这样可以保证上一帧里面有足够多的边缘点可以用来匹配，平面点也是一样
把每一条扫描线划分为6段，按照scan的顺序提取4种特征点，并对非边缘非平面点体素滤波降采样
曲率大的前2个点为边缘点，曲率小的前4个点为平面点，相邻5个点不重复选取
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)// 如果该点没有被选择过，并且曲率大于0.1
统计前五个后五个点，与当前点距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布
2.2 laserOdometry.cpp
帧键匹配：通过读取scanRegistration.cpp中的信息来计算帧与帧之间的变化，最终得到里程计坐标系下的激光雷达的位姿
步骤1：主要就是配置订阅和发布节点，以及一些提前使用的变量的初始化。
步骤2，步骤3：主要判断是否存在数据异常。
步骤4：用来判断是否有第一帧数据，如果没有第一帧数据，没有办法进行后续的匹配。所以需要进行一个初始化判断。
步骤5：是最关键的一个步骤，内部流程是将获取的每一个角点和面点（小集合）利用估计的映射，映射到上一时刻（这一帧雷达的启始时刻）（代码中有实现关于畸变去除的部分），通过利用历史帧构建的面KDTree和线KDTree（大集合）来匹配相似的点来构建点线和点面残差，利用ceres来求解较为精准的里程计。
TransformToStart
这个函数的运动补偿仅仅是最近邻查找的时候使用，在优化求解雷达位姿时是在ceres计算残差时进行运动补偿
把此帧的点云点补偿到起始时刻，如果有IMU，我们通过IMU得到的雷达高频位姿，可以求出每个点相对起始点的位姿，就可以补偿回去。
如果没有IMU，我们可以使用匀速模型假设，使⽤上⼀个帧间⾥程记的结果作为当前两帧之间的运动，假设当前帧也是匀速运动，可以估计出每个点相对起始时刻的位姿。
最后，当前点云中的点相对第一个点去除因运动产生的畸变，效果相当于静止扫描得到的点云。
// 基于匀速运动模型的点去畸变，将当前帧的点投影到当前帧起始时刻
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio差值比 0-1的数
double s;
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;//intensity存储的是ScanID + 时间戳差值
else
s = 1.0;// 由于kitti数据集上的lidar已经做过了运动补偿
//note: slerp四元数球面插值函数，q_last_curr是上一帧优化后的位姿变化，这里做的匀速运动假设
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
Eigen::Vector3d t_point_last = s * t_last_curr;//note: 平移的插值直接使用线性插值
Eigen::Vector3d point(pi->x, pi->y, pi->z);
Eigen::Vector3d un_point = q_point_last * point + t_point_last;//去畸变之后的点
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;
}
添加参数块
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);[]()
ceres添加边的残差
KD-tree最近邻点查找
先找最近邻的点A
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
再找与A不在同一条扫描线上的最近点B，这里查找B点在后面扫描线和前面扫描线都找了，最终选取最近的点
使用自动动求导，定义struct LidarEdgeFactor结构体，重载()函数,残差形式是三维的向量X,Y,Z
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
ceres添加面的残差
面的残差形式是一维的
//todo: 添加边的残差
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++;
KD-tree更新
是将此帧的次边缘点（包含边缘点），非特征点(包含平面点)存入kd-tree中，
kdtreeCornerLast->setInputCloud(laserCloudCornerLast)
KD-tree在输入点云时候会先清空原始数据，所以执行的是Scan-to-Scan的匹配
2.3 laserMapping.cpp
laserMapping.cpp主要是通过已经获得的激光里程计信息来消除激光里程计和地图之间的误差（也就是累计的误差），使得最终的姿态都是关于世界坐标的。
前端里程计会周期向后端发送位姿T_odom,但是Mapping模块中，我们需要得到位姿是T_map_cur,因此mapping就需要估计出odom坐标系与map坐标系之间的相对位姿变换T_map_odm，这部分需要仔细推导一下。 T_map_cur = T_map_odom × T_odom_cur
// Step: 1数据读取与时间戳判定，格式转换
// Step: 2地图管理，移动cube使得当前位姿不落在cube边缘
// Step: 3获取以当前位姿为中心的submap
// Step: 4ceres优化求解,使用点到submap的匹配，5个点求线与面
// Step: 5将当前帧的特征点云，逐点进行操作：转换到world坐标系并添加到对应位置的cube中并发布数据
图片来自于知乎@苍凉之悟的博客
坐标系转换
这里涉及到几个坐标系的相互转换，需要理清关系：雷达坐标系（每帧扫描到的点云点的坐标point_curr都在雷达坐标系中），里程计坐标系（由laseOdometry节点粗估计得到的LiDAR位姿wodom_curr对应的坐标系），地图坐标系（真实的世界坐标系）
map与odom在起始时是重合的，建图时，一般以初始的odom作为map
关键在于odom-base_link、map-base_link、map-odom 这三者间的关系
首先 odom-base_link 这个是由SLAM中的前端计算得到的，可以是视觉里程计、雷达里程计、编码器里程计、IMU里程计等方式获得的
其次到了 map-base_link ，这个是由全局定位，例如AMCL获得的，在ORB或者LOAM系列中，这个全局定位是由后端经过优化获得的
最后到了 map-odom ，在这里把 map-base_link 的 tf 当作真值，而 odom-base_link 的 tf’ 存在累计误差，那么就获得了 map-odom 的 TF = tf - tf’ ，前端计算没有误差的话这个TF=0
反正这个odom坐标系就可以理解为是一个为了看前端计算误差搞出来的虚拟坐标系，毕竟我们的惯性思维都是点在地图中哪个位置，当odom在抖动时也证明前端计算的位姿在抖动
地图管理
在原始LOAM中，使⽤的是基于珊格的地图存储⽅式。具体来说，将整个地图分成21×21×11个珊格，每个珊格是⼀个边⻓50m的正⽅体，当地图逐渐累加时，珊格之外的部分就被舍弃
珊格地图也需要随着当前位姿动态调整，从⽽保证我们可以从珊格地图中取出离当前位姿⽐较近的点云来进⾏scan-to-map算法，借以获得最优位姿估计
当当前位姿即将到达地图边界的时候，当前珊格地图就必须动态调整，当前帧即将抵达地图的左边界时，我们需要把当前帧和地图整体右移⼀部分，保证当前帧处于⼀个安全的位置
在A-LOAM中的栅格地图总大小为21 * 21 * 11，局部栅格地图为10 * 10 * 5，这里的单位是栅格，一个栅格的大小为50米
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 10;
int laserCloudCenDepth = 5;
const int laserCloudWidth = 21;
const int laserCloudHeight = 21;
const int laserCloudDepth = 11;
世界坐标系下的点云地图是固定的，但是删格的IJK坐标系是可以移动的，所以这6个while loop 的作用就是调整IJK坐标系（也就是调整所有cube位置），使得五当前帧在IJK坐标系的坐标范围处于 3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18，目的是为了防止后续向 四周拓展cube时，index成为负数。
PS:如果当前位姿所在的栅格位于左边界，则把栅格数据向右边移动一个单位，当前位姿所处栅格也向右移动一个单位，保证下次来的数据在坐标会被包含上
scan-to-submap
如果当前珊格centerCubeI索引小于3,就说明当前点快接近地图边界了，需要进行调整，相当于地图整体往x正方向移动，反方向同理，y，z方向同理，操作见前面的一维描述，最后附的代码也可以看到，以上操作相当于维护了一个局部地图，保证当前帧不在这个局部地图的边缘，这样才可以从地图中获取足够的约束
选取一定范围的点云，
先取到当前位姿所在的栅格，以当前格子为中心，x、y方向是各自前后取2个栅格，即x，y方向加上本身共有5个栅格，z方向上下各取1各栅格，则共有3个栅格，此时栅格大小为5 * 5 * 3，一个栅格为50米，则为250米 * 250米 * 150米 大小的小局部地图
开始构建用来这一帧优化的小的局部地图，用格子索引提取出对应格子里面的角点与面点，构成一个小的局部地图submap并构造KD-tree
// 向IJ坐标轴的正负方向各拓展2个cube，K坐标轴的正负方向各拓展1个cube，上图中五角星所在的蓝色cube就是当前位置
// 所处的cube，拓展的cube就是黄色的cube，这些cube就是submap的范围
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
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)// 如果坐标合法
{
// 记录submap中的所有cube的index，记为有效index
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
ceres优化
点到边的距离是在submap中取五个点用协方差矩阵分析(PCA分析)，对5个点的协方差矩阵求特征值与特征向量，如果最大特征值大于3倍次大特征值，则可以认为这几个点在一条线上，最大特征值对应的特征向量即为线的方向向量
// 五个点的去质心坐标的协方差矩阵
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
for (int j = 0; j < 5; j++)
{
Eigen::Matrix tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
// 计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布，此为PCA的原理
Eigen::SelfAdjointEigenSolver saes(covMat);
// 如果最大的特征值 >> 3倍次大特征值，则5个点确实呈线状分布，否则认为直线“不够直”
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 从中心点沿着方向向量向两端移动0.1m，构造线上的两个点
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
点到面的距离，直接使用AX+BY+CZ=-1的平面方程求解平面法向量，5个点构成超定方程，使用QR分解求解，使用点到面的距离判断面是否拟合好，以及构建点到面的残差方程
地图更新位姿发布
将当前帧的特征点云，逐点进行操作：转换到world坐标系并添加到对应位置的cube中
高频位姿发布，在接收里程计位姿的回调函数里发布高频位姿，用mappping里面估计的里程计与世界坐标系之间的漂移来将里程计输出位姿转移到世界坐标系下发布，与里程计信息发布频率一样
接收里程计输出的位姿信息，存入数据，然后用优化后的里程计的漂移修正位姿再发布出去，接收一次发布一次
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
低频位姿发布，只发布每次优化完的当前帧的位姿
pubOdomAftMapped.publish(odomAftMapped);
3、LOAM源码
LOAM源码主要由四个节点构成，分别完成特征点提取，高频低精度odom， 低频高精度odom， 双频odom融合的功能，每个节点以rosnode的形式存在， 也就是说是独立的进程，进程间通过rostopic传递点云， odom等数据。
坐标系
imu为x轴向前,y轴向左,z轴向上的右手坐标系，
velodyne lidar被安装为x轴向前,y轴向左,z轴向上的右手坐标系，
scanRegistration会把两者通过交换坐标轴，都统一到z轴向前,x轴向左,y轴向上的右手坐标系
3.1 ScanRegistration
针对单个Scan提取特征点, 而相对时间会在laserOdometry中用于运动补偿.所有Scan的特征点,拼到两个点云中(因为是corner和surface两种特征点,所以是两个点云).至此,每帧点云,输出两帧对应的特征点云, 给下一个节点laserOdometry。
点云回调函数
Step: 1点云的点转化为16线，根据VLP16的激光扫描模型, 对单帧点云（paper中称为一个Sweep）进行分线束（分为16束）, 每束称为一个Scan， 并记录每个点所属线束和每个点在此帧点云内的相对扫描时间（相对于本帧第一个点）。
Step: 2如果有IMU数据，则使用imu数据进行加速度畸变矫正
Step: 3计算点云曲率并筛选出边缘点，平行点
//1 平行点筛选，如果相邻两点夹角小于0.1则认为是平行点
sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1
//2 与前后点的距离平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
cloudNeighborPicked[i] = 1;//筛选的点试讲picked数组置为1
Step: 4特征点提取，冒泡排序，选择曲率最大的两个点为边缘点，4个平面点，一个也是四种点，边缘点，次边缘点(包含边缘点)，平面点，次平面点（包含平面点），也是前后5个点不连续选取，A-loam和他一样
IMU回调函数
接收imu消息，直接获取四元数作为此时的姿态，进行速度位移积分imu坐标系为x轴向前，y轴向右，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
//将IMU四元数消息转换为四元数
tf::quaternionMsgToTF(imuIn->orientation, orientation);
//这将分别从矩阵中获得关于固定轴X、Y、Z的滚转俯仰和偏航。这就是R=Rz（偏航）*Ry（俯仰）*Rx（滚转）。
//此处横摇俯仰偏航位于全局坐标系中
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
//减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
//循环移位效果，形成环形数组
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
AccumulateIMUShift();//速度位移积分
}
3.2 laserOdometry
实现运动补偿和帧间配准.每帧激光都会参与（所以帧率同VLP16的扫描帧率，10hz）.通过对每一帧激光的配准,可以得到一个精度较差的ODOM,帧与帧配准的初始POSE可以由IMU得到，或者在没有IMU的时候由匀速运动模型(简单的说就是假设这一帧运动量与上一帧一致)得到。
本节点输出的ODOM频率为10hz.此ODOM的作用:
（a） 在laserMapping中用于位姿的预测。
（b） 在transformMaintenance中为laserMapping输出的低频ODOM提供插值,以获得高频(10HZ)的ODOM.
在实际应用中,帧间匹配实现的ODOM,可以由IMU, 视觉里程计,底盘编码器等替代.而运动补偿在高速场景是不可或缺的。
//Step: 1数据接收与存储转换为PCL格式
//Step: 2初始化，将第一个点云数据集发送给laserMapping,从下一个点云数据开始处理
//Step: 3LM迭代求解,最多迭代25次,每迭代5次重新匹配点
for(i = 0; i < 25; ++i)
for(i% 5 = 0){//每迭代5次就重新匹配一次
//Step: 3.1 添加点到线的残差
//Step: 3.2 添加点到面的残差
}
//Step: 3.3 求增量方程
//Step: 3.4在第一次迭代的时候判断是否处于退化场景
//Step: 3.5 计算旋转平移量，如果很小就停止迭代
点到边的残差是三维的向量，残差分摊到各个轴上
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
点到面的残差也是三维的
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;//最终的距离残差
退化问题
https://zhuanlan.zhihu.com/p/258159552
在第一次迭代的时候判断是否处于退化场景，判断退化方法：JTJ的特征值是否太小，如果太小将此特征值对应的特征向量置为0，然后重新得到一个P矩阵，每次对优化变量左乘P矩阵
float eignThre[6] = {10, 10, 10, 10, 10, 10};
for (int i = 5; i >= 0; i--) {//从小到大查找
if (matE.at(0, i) < eignThre[i]) {//特征值太小，则认为处在兼并环境中，发生了退化
for (int j = 0; j < 6; j++) {//对应的特征向量置为0
matV2.at(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
//计算P矩阵
matP = matV.inv() * matV2;
}
if (isDegenerate) {//如果发生退化，只使用预测矩阵P计算
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
优化完成之后点云发布与kd-tree更新等等
3.3 laserMapping
本节点使用实现了一个较为完整的SLAM过程,也就是同时建图和定位.主要工作：
（a）通过将多帧的激光特征点云基于Pose拼接,形成特征点云地图，前面有提到特征包含corner和surface两种,那么这里就是建立了两种特征点云地图.
（b）将新入的帧与地图作配准,得到更精准的pose ，又基于这个pose执行a过程进行建图。
由于单帧与地图配准计算量较大,本节点没有将所有帧与地图进行配准,而是间隔几帧,比如每5帧配准一次（那么频率就是2hz）.如果你的应用场景运算性能完全够, 你可以一帧一匹配.如果这样, transformMaintenance这个节点就没有存在的意义了.
话题订阅：订阅/laser_cloud_corner_last和/laser_cloud_surf_last特征点云，订阅特征点云对应的全局位姿/laser_odom_to_init。然后订阅第二部分laserOdometry处理后的实时点云/velodyne_cloud_3，订阅/imu/data用于读取imu姿态角。最后发布/laser_cloud_surround和/velodyne_cloud_registered和/aft_mapped_to_init。
点云回调函数将点云rosmsgs格式转换为pcl格式，全局位姿回调将位姿信息保存在transformSum中，imu回调函数保存时间戳和roll/pitch信息。