第二天：ALOAM前端讲解【第3部分】

（2）面特征

点到面的距离公式：

$$d_{\mathcal{H}}=\frac{\left|\left({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,j)}^L\right)\cdot \left(\left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right)\times \left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,m)}^L\right)\right)\right|}{\left|\left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right)\times \left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,m)}^L\right)\right|}\text{(2)}$$

公式解释

这个公式计算点 ${\tilde{X}}_{(k+1,i)}^L$ 到平面的距离。平面由三点 ${\bar{X}}_{(k,j)}^L$，${\bar{X}}_{(k,l)}^L$ 和 ${\bar{X}}_{(k,m)}^L$ 确定。

1. 分子部分： $\left({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,j)}^L\right)\cdot \left(\left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right)\times \left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,m)}^L\right)\right)$

   • 计算点 ${\tilde{X}}_{(k+1,i)}^L$ 到平面上的一个点 ${\bar{X}}_{(k,j)}^L$ 的向量 $v3$，与平面法向量 $\mathbf{n}$ 的点积。
   • $\mathbf{n}$ 是由两个平面向量 $v1=\left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right)$ 和 v 2 = ( X ˉ ( k , j ) L − X ˉ (