学长论文中公式的推导

相机标定

针对第一章相机标定中的公式的推导详见我的另一篇博客：相机标定

Kalman Filter

圆柱包围盒碰撞检测：

空间中直线间的距离

考虑空间中， $AB$ 和 $CD$ 是两条异面直线，A、B、C、D四个点的坐标分为 $(X_A,Y_A,Z_A) \quad (X_B,Y_B,Z_B) \quad (X_C,Y_C,Z_C) \quad (X_D,Y_D,Z_D)$ 。记 $\overrightarrow{AB}$ 的方向向量为 $\overrightarrow{S_1}$ ， $\overrightarrow{CD}$ 的方向向量为 $\overrightarrow{S_2}$
则

$\overrightarrow{S_1} = (X_B - X_A, Y_B - Y_A, Z_B - Z_A)$

$\overrightarrow{S_2} = (X_D - X_C, Y_D - Y_C, Z_D - Z_C)$

记

$\begin{aligned} \overrightarrow{S_3} = \overrightarrow{S_1} \times \overrightarrow{S_2} & = (M, N, P) \\= \Big( &(Y_B-Y_A)\cdot(Z_D-Z_C)-(Z_B-Z_A)\cdot(Y_D-Y_C), \\ & (Z_B-Z_A)\cdot(X_D-X_C)-(X_B-X_A)\cdot(Z_D-Z_C),\\ & (X_B-X_A)\cdot(Y_D-Y_C)-(Y_B-Y_A)\cdot(X_D-X_C)\Big) \end{aligned}$

由平面的点法式方程（点 $A$ 和方向向量 $\overrightarrow{S_3}$ ）可得一个过直线 $AB$ 且平行 $CD$ 的平面 $\pi$ 。

$\pi$ 的方程为： $M(x-X_A)+N(y-Y_B)+P(z-Z_B) = 0$ ，将其化为一般形式 $Mx + Ny + Pz + Q= 0$ 可得，

$Q = - \begin{bmatrix} M,N,P \end{bmatrix} \cdot \begin{bmatrix} X_A \\ Y_A \\ Z_A \end{bmatrix}$

于是 $\pi$ 的方程写成矩阵形式：

$\begin{bmatrix} M,N,P \end{bmatrix} \cdot \begin{bmatrix} x-X_A \\ y-Y_A \\ z-Z_A \end{bmatrix} = 0$

则点 $C(X_C, Y_C, Z_C)$ 或点 $D(X_D, Y_D, Z_D)$ 到 $\pi$ 的距离即为异面直线 $AB$ 和 $CD$ 间的距离

$\begin{aligned} d &= \frac{|MX_C + NY_C + PZ_C + Q|}{\sqrt{M^2 + N^2 + P^2}}\\ & = \frac{(\overrightarrow{S_1} \times \overrightarrow{S_2})\cdot(\overrightarrow{AC})}{|\overrightarrow{S_1} \times \overrightarrow{S_2}|}\\ & = \frac{[\overrightarrow{S_1} , \overrightarrow{S_2},\overrightarrow{AC}]}{|\overrightarrow{S_1} \times \overrightarrow{S_2}|} \end{aligned}$

将具体坐标带入上式即得论文中公式(4-5)

两个线段共面时候的距离

平面上两条线段之间的最短距离定义：

两线段 $AB$ 和 $CD$ 用其端点 $(A,B)$ ， $(C,D)$ 表示

$AB$ , $CD$ 上距离最近的两点之间的距离 $d(A,B)$ ，分为三种情况讨论。

若两线段有交点，距离d为0
计算点A和点B到CD线段所在直线的距离，记为 $l_1$ 和 $l_2$ ；计算点C和点D到AB线段所在直线的距离，记为 $l_3$ 和 $l_4$ 。

$d(A,l_2)$ , $d(B,l_2)$ , $d(C,l_3)$ ， $d(D,l_4)$

选择距离最小，且垂足落在对方线段内的，作为 $d(A,B)$
若四个垂足都落在对方线段外，计算四个端点两两之间的距离，取最小值作为 $d(A,B)$