LeGO-LOAM中的数学公式推导

LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法，建图效果比LOAM要好，但是建图较为稀疏，计算量也更小了。

本文原地址：wykxwyc的博客

github注释后LeGO-LOAM源码：LeGO-LOAM_NOTED
关于代码的详细理解，建议阅读：

1.地图优化代码理解

2.图像重投影代码理解

3.特征关联代码理解

4.LeGO-LOAM中的数学公式推导

以上博客会随时更新，如果对你有帮助，请点击注释代码的github仓库右上角star按钮，你的鼓励将给我更多动力。

本文 记录重新读LeGO-LOAM的代码时看到的数学运算，记录这些数学运算背后的原理，会随时更新。

featureAssociation中的数学公式

To be continued.

mapOptmization中的数学公式

cornerOptimization中的协方差矩阵计算

随机变量的协方差是什么?
1.在概率论和统计中，协方差是对两个随机变量联合分布线性相关程度的一种度量。两个随机变量越线性相关，协方差越大，完全线性无关，协方差为零。（线性无关并不代表完全无关，更不代表相互独立。）

2.两个随机变量$X$与$Y$协方差的定义如下：
$$\mathrm{cov}(X,Y)=\mathrm{E}[(X-\mathrm{E}[X])(Y-\mathrm{E}[Y])]\text{\hspace{1em}(CO-1)}$$

如何判断两个随机变量的相关程度？
1.通过定义这两个变量之间的相关系数$\eta$进行判断：
$$\eta =\frac{\mathrm{cov}(X,Y)}{\sqrt{\mathrm{var}(X)\cdot \mathrm{var}(Y)}}\text{\hspace{1em}(CO-2)}$$

1表示完全线性相关，−1表示完全线性负相关，0表示线性无关。线性无关并不代表完全无关，更不代表相互独立。

样本的协方差矩阵
1.设多维随机变量$\mathbf{X}={\left[X_1,X_2,X_3,\dots ,X_n\right]}^T$的协方差矩阵为$\Sigma$，则协方差矩阵中的每个元素为:
$${\Sigma }_{ij}=\mathrm{cov}\left(X_i,X_j\right)=\mathrm{E}\left[\left(X_i-\mathrm{E}\left[X_i\right]\right)\left(X_j-\mathrm{E}\left[X_j\right]\right)\right]\text{\hspace{1em}(CO-3)}$$
上式表示的是$X_i$和$X_j$之间的协方差。

公式CO-3也揭示了协方差矩阵中每个元素的计算过程，整个矩阵为：
Σ = E [ ( X − E [ X ] ) ( X − E [ X ] ) T ] = [ cov ⁡ ( X 1 , X 1 ) cov ⁡ ( X 1 , X 2 ) … cov ⁡ ( X 1 , X n ) cov ⁡ ( X 2 , X 1 ) cov ⁡ ( X 2 , X 2 ) … cov ⁡ ( X 2 , X n ) ⋮ ⋮ ⋱ ⋮ cov ⁡ ( X n , X 1 ) cov ⁡ ( X n , X 2 ) ⋯ cov ⁡ ( X n , X n ) ] = [ E [ ( X 1 − E [ X 1 ] ) ( X 1 − E [ X 1 ] ) ] E [ ( X 1 − E [ X 1 ] ) ( X 2 − E [ X 2 ] ) ] ⋯ E [ ( X 1 − E [ X 1 ] ) ( X n − E [ X n ] ) ] E [ ( X 2 − E [ X 2 ] ) ( X 1 − E [ X 1 ] ) ] E [ ( X 2 − E [ X 2 ] ) ( X 2 − E [ X 2 ] ) ] ⋯ E [ ( X 2 − E [ X 2 ] ) ( X n − E [ X n ] ) ] ⋮ ⋮ ⋱ ⋮ E [ ( X n − E [ X n ] ) ( X 1 − E [ X 1 ] ) ] E [ ( X n − E [ X n ] ) ( X 2 − E [ X 2 ] ) ] ⋯ E [ ( X n − E [ X n ] ) ( X n − E [ X n ] ) ] ] \begin{aligned} &\Sigma =\mathrm{E}\left[(\mathbf{X}-\mathrm{E}[\mathbf{X}])(\mathbf{X}-\mathrm{E}[\mathbf{X}])^{T}\right] \\ &=\left[\begin{array}{cccc}{\operatorname{cov}\left(X_{1}, X_{1}\right)} & {\operatorname{cov}\left(X_{1}, X_{2}\right)} & {\dots} & {\operatorname{cov}\left(X_{1}, X_{n}\right)} \\ {\operatorname{cov}\left(X_{2}, X_{1}\right)} & {\operatorname{cov}\left(X_{2}, X_{2}\right)} & {\dots} & {\operatorname{cov}\left(X_{2}, X_{n}\right)} \\ {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {\operatorname{cov}\left(X_{n}, X_{1}\right)} & {\operatorname{cov}\left(X_{n}, X_{2}\right)} & {\cdots} & {\operatorname{cov}\left(X_{n}, X_{n}\right)}\end{array}\right] \\ &=\left[\begin{array}{cccc}{\mathrm{E}\left[\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\right]} & {\mathrm{E}\left[\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\right]} & {\cdots} & {\mathrm{E}\left[\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\right]} \\ {\mathrm{E}\left[\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\right]} & {\mathrm{E}\left[\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\right]} & {\cdots} & {\mathrm{E}\left[\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\right]} \\ {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {\mathrm{E}\left[\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\left(X_{1}-\mathrm{E}\left[X_{1}\right]\right)\right]} & {\mathrm{E}\left[\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\left(X_{2}-\mathrm{E}\left[X_{2}\right]\right)\right]} & {\cdots} & {\mathrm{E}\left[\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\left(X_{n}-\mathrm{E}\left[X_{n}\right]\right)\right]}\end{array}\right] \end{aligned} ​Σ=E[(X−E[X])(X−E[X])T]=⎣⎢⎢⎢⎡​cov(X1​,X1​)cov(X2​,X1​)⋮cov(Xn​,X1​)​cov(X1​,X2​)cov(X2​,X2​)⋮cov(Xn​,X2​)​……⋱⋯​cov(X1​,Xn​)cov(X2​,Xn​)⋮cov(Xn​,Xn​)​⎦⎥⎥⎥⎤​=⎣⎢⎢⎢⎡​E[(X1​−E[X1​])(X1​−E[X1​])]E[(X2​−E[X2​])(X1​−E[X1​])]⋮E[(Xn​−E[Xn​])(X1​−E[X1​])]​E[(X1​−E[X1​])(X2​−E[X2​])]E[(X2​−E[X2​])(X2​−E[X2​])]⋮E[(Xn​−E[Xn​])(X2​−E[X2​])]​⋯⋯⋱⋯​E[(X1​−E[X