特征提取 Lidar Registration：

众所周知，为了计算雷达帧间的姿态变换，使用全部激光数据是不现实的，为了减少计算量，提出了特征点代替全部点云的方案。

作者提出量一种根据点的曲率计算平面光滑度的方法提取特征点.

具体到代码上是通过判断点\(X_{(k,j)}^L\)前后5个点的均值来计算平滑度。

得到平滑度之后通过一下标准将数据分为平面点和边缘点。

平面点：在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。
边缘点：在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。

同时，选取特征点也有一定的限制：

该点的周围的点尽量不要被再被取到，这样可以使整体的特征点分布更加的平均
该点不能与雷达扫描束过于平行，这样也会出现问题

里程计 Odometry：

提取特征点之后就是特征匹配，并使用scan-to-scan的方法计算里程计。当然也是分为平面点和边缘点。

定义第\(k\)次扫描点云为\(P_k\),提取的边缘点为\(E_k\)，平面点为\(H_k\)，去畸变之后的边缘点和平面点分别为\(\hat{E_k},\hat{H_k}\)。

边缘点

对于边缘点，就是三维结构信息中的线，因此可以构建评价函数为点到线的距离。因此需要匹配\(E_k\)中的一点在\(\hat{E_{k+1}}\)中的最近不共线的两点。

\[ d_e = \frac{|(\hat{X_{(k+1,i)}} -X_{(k,j)}) \times (\hat{X_{(k+1,i)}} - X_{(k,l)})|}{|X_{(k,j)} - X_{(k,l)}|} \]

其中，分母为向量\(\vec{ij},\vec{il}\)构成平行四边形面积，分母\(\vec{jl}\)构成底，因此高等于面积除以底。

平面点

类似的，对于平面点可以构建评价函数为点到面的距离。因此需要匹配\(H_k\)中的一点在\(\hat{H_{k+1}}\)中最近不公面的三点。

\[ d_e = \frac{| (\hat{X_{k+1, i}} - X_{k,j}) (X_{(k,j)} - X_{(k,l)}) \times (X_{(k,j)} - X_{(k,m)})|}{|(X_{(k,j)} - X_{(k,l)}) \times (X_{(k,j)} - X_{(k,m)})|} \]

其中分子为构成的三维物体的体积，分母为地面构成的平行四边形的面积。

因此可构建最小二乘如下：

\[ \min _{\mathcal{X}} \left\{\sum_{i \in {\hat{E_{k+1}}}, j,l \in{E_k}}\left\| \frac{|(\hat{X_{(k+1,i)}} -X_{(k,j)}) \times (\hat{X_{(k+1,i)}} - X_{(k,l)})|}{|X_{(k,j)} - X_{(k,l)}|} \right\|_{}^{2} + \sum_{i \in {\hat{H_{k+1}}},j,l,m \in{H_k}} \left\| \frac{| (\hat{X_{k+1, i}} - X_{k,j}) (X_{(k,j)} - X_{(k,l)}) \times (X_{(k,j)} - X_{(k,m)})|}{|(X_{(k,j)} - X_{(k,l)}) \times (X_{(k,j)} - X_{(k,m)})|} \right\|^2 \right\} \]

建图 Mapping：

Reference

https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf