留个笔记自用
Efficient LiDAR Odometry for Autonomous Driving
做什么
Lidar Odometry激光雷达里程计，里程计作为移动机器人相对定位的有效传感器，为机器人提供了实时的位姿信息。移动机器人里程计模型决定于移动机器人结构和运动方式，即移动机器人运动学模型。
简单来说，里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法
用建图的方式来理解，要实现机器人的定位与导航，就需要知道机器人走了多远，往哪走，也就是初始位姿和终点位姿，只有知道了里程计，才能准确将机器人扫描出来的数据进行构建。
做了什么
这里利用了非地面的球面距离图像和地面的鸟瞰图结合的方法，并且提出了一种自适应法向量估计方法，解决了投影图像时存在的相似点失去高度信息的问题。
首先先要了解一下什么叫地面点，这里的激光雷达里程计是基于自动驾驶的
以汽车为基础的雷达就会出现一个问题，往往方向都是朝向地面的，因此大部分扫描的点云都是属于路面的，这会导致姿态估计的偏移，而本文就是将这两种点进行有效融合
怎么做
首先是任务设计，这里将任务设计成了一个非线性最小二乘最小化问题，激光雷达里程计的任务是通过输入点云Lt和Lt-1，希望得到位姿转换Ttt-1∈SE（3）。。
这里构造了两个不同的cost，地面cost和非地面cost并进行分别处理，其实也可以理解成这俩是两个loss
这里的Es是非地面cost，EG是地面cost
很显然上面这个式子是一个个非线性最小二乘最小化问题，可以进行最小化优化
总的来说，整体结构分为四步，输入是原始3D点云集，第一步首先通过柱面投影的方式将点云投影至2D平面，以快速得到分割结果和非地面cost。第二步然后通过得到的平面分割出地面点，将其进一步投影到2D鸟瞰图上形成地面cost。第三步通过距离自适应算法计算2D平面的法向量图，然后通过ICP计算位姿转换。第四步最后通过投影map和鸟瞰map更新地图。
首先是第一步，也就是所有LO里都有的点云输入处理问题，这里也采用了柱面投影的方式，这里展示了一下前面所述的常见投影方式会遇到的问题
在这里，G1、G2、G3是地面点，F1、F2、F3是表面点，也就是所说的非地面点。
上排的Range Image就是柱面投影的方式，柱面投影存在的问题很明显，缺少了点云的高度信息，它们会将投影在相近列中，就算包含了点的角度等信息，也会容易存在冗余重复的现象。除了非地面点遇到的问题，地面点G也存在问题，由于三个地面点其实角度上差距较小，投影容易较为接近，就算它们之间的像素距离可能相差特别大。
这里就采用了鸟瞰图的方法来包含处理点云的地面信息
虽然在这里F也就是非地面点的高度问题没有解决，但很好地解决了地面信息也就是G地面点的问题，使地面点之间的距离关系最大程度地进行了保留，至于F的高度问题则在后面部分进行融合解决。
然后是投影的具体公示
这里的（x，y，z）是点的三维坐标r表示点的范围，θ表示方位角，φ代表仰角分量，这三个都是点的特征，w和h是人为给定的呃参数，用来限制投影出来的平面大小，f是雷达的探测范围大小
至此得到2D平面，每个点（x，y，z）索引至坐标（u，v）
然后是第二步，地面cost Es，这个loss是由2D投影平面计算得到，可以理解成为了那些非地面点而进行的优化
这里采用的方式是scan to model的方式，VD在scan层次上顶点的对应：R2->R3，相应的法向量图对应是ND，这在后面有用到。VM是model层次上顶点的对应，NM是相应的法向量图对应
其中
u即是属于VD中的一个顶点，这里的Π是柱面投影函数，vu是属于VM的一个顶点，nu是一个法向量
简单来说，这里的ES的意思就是常见的对比loss，将点根据得到的位姿转换T进行转换，希望投影误差尽可能减小，以此估计姿态相对变换，是一个帧与模型的配准问题
然后是第三步，通过一个自适应的方法计算平面法向量图
常见的平面可以用一个函数进行代表，nxx+nyy+nzz-d=0，于是可以定义各部分的法向量误差e
这里的pi是3D点坐标，是总点云集的一个小子集（由于法向量是需要局部平面进行计算），n是（nx，ny，nz）也就是该平面的法向量，d是平面偏移量
为了优化这个值，采用SVD计算局部点的协方差矩阵
这个3×3特征矩阵的最小特征值对应的特征向量就作为这个局部的法向量，也可以采用一点加权的因素
接下来是设计前面的局部区域pi邻域的大小，这也会影响逐点的法向量计算
这里的ws和hs是投影图片的分辨率，l和y是规定的local大小。
至此，使用自适应法向量计算得到了非地面点的cost ES
然后是第四步，根据地面分割和鸟瞰点计算地面点cost EG
首先是地面点分割，也就是为了知道点里有哪些是地面点，直观理解中，所谓的地面点，在邻域中应存在两个特性，第一个是地面点和雷达中心的距离应该较大，第二个就是相邻点之间的竖直角度差应该较小，因为都是地面平行的
所以第一个规定
这里的hp是点的z轴高度，hg是激光雷达的中心高度，后面的两个是人为规定的参数，意思就是过滤掉一些在激光雷达扫描中心的非地面点
然后是第二个规定
这里借用了平面进行辅助，p是柱面上某一点的坐标，pup和pdown分别是p点的邻域，也就是相对于点（u，v）的
然后要求这里的两个θ都要小于规定的阈值 δ
简单来说，这里就是控制相邻点的角度差，如果小的话说明可能是地面点
至此，通过投影平面得到了地面点的分割图
然后是BEV俯瞰图
这里的W和B和S均是人为规定的参数，代表了俯瞰图的横纵大小范围和分辨率
然后就可以定义地面cost EG了
同样，这也是个配准问题，也是scan-to-model的做法，BG是一个R2->R3的映射函数，可视为快速最近邻搜索的2D搜索表，其中每个像素包含3D地面点，BM是model层次上的
这里的做法跟前面的非地面点非常相似，都类似于对比LOSS进行一个配准
最后就是优化之后的模型更新方案，应用了前面的投影柱面和俯瞰图
对于非地面的柱面投影，维护一张顶点图VM和法向量图NM，这都是计算非地面cost层次中model层次的。更新的方法是对于3D点用位姿转换T进行转换，然后再进行投影，计算两个投影的距离。
对于地面点只维护model层次的顶点图BM，跟上面方法类似。
总结
1.全程看下来很多地方看的不是很懂，感觉这种模型更新方法很怪，希望有机会能阅读以下这篇的代码
2.感觉较有收获的点还是处理数据的方式，柱面投影的问题在这里得到了一种简单解决，鸟瞰图判断地面点确实能做到这一点
