LinePlane intersection && Plane Parameterization

三维空间直线与平面的交点计算与平面方程优化的参数化方法。

1. 线面交点计算

线面交点计算方法有很多种，列出两种，使用何种方法与线面表达的形式有关（形式可以转换，这种关系只涉及便利程度）。

1.1. 方法一

问题描述：有一平面，法向为 \(\mathbf{n}\) ，平面上一点 \(\mathbf{X}_0\)；有一直线，方向为 \(\mathbf{d}\)，直线上一点 \(\mathbf{X}_1\)。求直线与平面的交点。

解：

先判断是否有交点。计算 \(\mathbf{n}\) 与 \(\mathbf{d}\)，如果绝对值小于某值则两者之间的夹角足够接近于 90°（考虑数值精度，不是完全等于 90°），无交点。

否则有交点。

假设交点为 \(\mathbf{X}\)。那么有直线 \(\mathbf{X}_1\mathbf{X}\) 与方向 \(\mathbf{d}\) 平行，直线 \(\mathbf{X}_0\mathbf{X}\) 与方向 \(\mathbf{n}\) 垂直。
写成线性方程如下。

\[\begin{align}(\mathbf{X} - \mathbf{X}_0)^{T} \mathbf{n} &= 0 \\ (\mathbf{X} - \mathbf{X}_1) \times \mathbf{d}&= \mathbf{0} \end{align} \]

整理得。

\[\begin{align}\begin{bmatrix} \mathbf{n}^T \\ [\mathbf{d}_{\times}] \end{bmatrix} \mathbf{X} = \begin{bmatrix} \mathbf{n}^T\mathbf{X}_0 \\ [\mathbf{d}_{\times}]\mathbf{X}_1 \end{bmatrix}\end{align} \]

解之即得（\([\mathbf{d}_{\times}]\) 的秩只有 2，所以以上 4 个方程的后 3 个仅需取 2 个，3 个方程解 3 个未知数能够保证只有 1 个或 0 个解，因为前面已经做了向量方向判断，排除 0 个解的情况，剩下 1 个解的情况）。

1.2. 方法二

考虑到希望优化线与平面方程，需要计算导数，以上交点 \(\mathbf{X}\) 的结果是隐式的，无法计算导数。

问题描述：有一平面，法向为 \(\mathbf{n}\) ，平面上一点 \(\mathbf{X}_0\)；有一直线，方向为 \(\mathbf{d}\)，直线上一点 \(\mathbf{X}_1\)。求直线与平面的交点。

平面方程可以用 \(\mathbf{\pi}\) 表示（\(\mathbf{\pi}\mathbf{X}=AX+BY+CZ+D=0\)）。\(\mathbf{\pi}\) 与 \(\mathbf{n}, \mathbf{X_0}\) 的关系如下。

\[\begin{align}\mathbf{\pi} = \begin{bmatrix} \mathbf{n} \\ -\mathbf{n}^T \mathbf{X}_0\end{bmatrix} = \begin{bmatrix} n_x \\ n_y \\ n_z \\ -n_xX_0 - n_yY_0 - n_zZ_0\end{bmatrix}\end{align} \]

直线方程可以用 Plucker Matrix \(\mathbf{L}\) 表示。\(\mathbf{L}\) 与 \(\mathbf{X}, \mathbf{d}\) 之间的关系如下。（Plucker Matrix 参考 [1] 3.2 Representing and transforming planes, lines and quadrics Page 72）

\[\begin{align} \mathbf{L} = \mathbf{X}_1\mathbf{d}^T - \mathbf{d}\mathbf{X}_1^T \end{align} \]

直线 \(\mathbf{L}\) 与平面 \(\mathbf{\pi}\) 的交点 \(\mathbf{X}\) （齐次）如下。

\[\begin{align}\mathbf{X} = \mathbf{L} \mathbf{\pi}\end{align} \]

2. 导数计算

按照方法二计算导数。

为方便区分齐次与非齐次，令。

\[\begin{align}\mathbf{\pi} = \begin{bmatrix} \mathbf{\Pi} \\ \pi \end{bmatrix}\end{align} \]

整理交点 \(\mathbf{X}\) 方程（符号不容易写，请自行分辨一下齐次与非齐次坐标）。

\[\begin{align} \mathbf{X} &= (\mathbf{X}_1\mathbf{d}^T - \mathbf{d}\mathbf{X}_1^T) \mathbf{\pi} \\ &= \mathbf{X}_1\mathbf{d}^T\mathbf{\pi} - \mathbf{d}\mathbf{X}_1^T \mathbf{\pi} \\ &= (\mathbf{d}^T\mathbf{\pi}\mathbf{I} - \mathbf{d}\mathbf{\pi}^T)\mathbf{X}_1 \\ &= \left( \begin{bmatrix} \mathbf{d}^T & 0 \end{bmatrix} \begin{bmatrix} \mathbf{\Pi} \\ \pi \end{bmatrix} \mathbf{I} - \begin{bmatrix} \mathbf{d} \\ 0 \end{bmatrix} \begin{bmatrix} \mathbf{\Pi}^T & \pi \end{bmatrix} \right) \begin{bmatrix} \mathbf{X}_1 \\ 1 \end{bmatrix} \\ &= \left( \mathbf{d}^T\mathbf{\Pi}\mathbf{I} - \begin{bmatrix} \mathbf{d}\mathbf{\Pi}^T & \mathbf{d}\pi \\ \mathbf{0}^T & 0 \end{bmatrix} \right) \begin{bmatrix} \mathbf{X}_1 \\ 1 \end{bmatrix} \\ &= \begin{bmatrix} \mathbf{d}^T\mathbf{\Pi}\mathbf{I} - \mathbf{d}\mathbf{\Pi}^T & -\mathbf{d}\pi \\ \mathbf{0}^T & \mathbf{d}^T\mathbf{\Pi} \end{bmatrix} \begin{bmatrix} \mathbf{X}_1 \\ 1 \end{bmatrix} \\ &= \begin{bmatrix} (\mathbf{d}^T\mathbf{\Pi}\mathbf{I} - \mathbf{d}\mathbf{\Pi}^T) \mathbf{X}_1 - \pi \mathbf{d} \\ \mathbf{d}^T\mathbf{\Pi} \end{bmatrix} \end{align}\]

非齐次坐标。

\[\begin{align} \mathbf{X} = (\mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \mathbf{d}^T \mathbf{\Pi}}) \mathbf{X}_1 - {\pi \mathbf{d} \over \mathbf{d}^T \mathbf{\Pi}} \end{align}\]

思考直线与平面的自由度的个数，做好计算导数的准备。

在二维空间中直线与点是对偶的，各自有 3 个自由度。在三维空间中平面与点是对偶的，各自有的 3 个自由度。理解三维平面有 3 个自由度可以有两种方式：1. 用截距式表达\({X \over A} + {Y \over B} + {Z \over C} = 1\)，用平面与三轴的交点表达平面方程；2. 用平面的法向量与平面距离原点的距离表达，三维空间方向向量的自由度为 2，距离自由度为 1。

在三维空间中的直线：1. 表达为一个经过的点与方向向量，点的自由度为 3，方向向量自由度为 2，一共 5 个自由度；2. 表达为两个平面的交点，两个平面的自由度为 6，两个平面的 range space 无重合，则两个平面不相交，两个平面的 range space 有 1 维重合，两个平面相交于一条线，直线自由度为 6 - 1 = 5。

计算导数的时候，直线方程用点 \(\mathbf{X}_1\)、方向 \(\mathbf{d}\) 表达，平面方程用 \(\mathbf{\pi}\) 表达，计算导数就是计算 \(\mathbf{X}\) 对 \(\mathbf{X}_1, \mathbf{d}, \mathbf{\pi}\) 的导数。

先计算两个简单的。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{X}_1} &= \mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \mathbf{d}^T \mathbf{\Pi}} \\ {\partial \mathbf{X} \over \partial \pi} &= - {\mathbf{d} \over \mathbf{d}^T \mathbf{\Pi}} \end{align}\]

计算对 \(\mathbf{d}\) 的导数。

变换 \(\mathbf{X}\) 方程的形式，令 \(\lambda=\mathbf{d}^T \mathbf{\Pi}\)。

\[\begin{align} \mathbf{X} &= (\mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \mathbf{d}^T \mathbf{\Pi}}) \mathbf{X}_1 - {\pi \mathbf{d} \over \mathbf{d}^T \mathbf{\Pi}} \\ &= -{\mathbf{\Pi}^T\mathbf{X}_1 \over \lambda} \mathbf{d} - {\pi \over \lambda} \mathbf{d} + \mathbf{X}_1 \\ &= (-\mathbf{\Pi}^T\mathbf{X}_1 - \pi){\mathbf{d}\over\lambda} + \mathbf{X}_1 \end{align}\]

每个 element 分别计算。

\[\begin{align} {\mathbf{d} \over \lambda} &= \begin{bmatrix} {d_1 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3} \\ {d_2 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3} \\ {d_3 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3} \end{bmatrix} \\ {\partial {\mathbf{d} \over \lambda}\over \partial \mathbf{d}} &= {1 \over \lambda}(\mathbf{I} - {1\over\lambda}\mathbf{d}\mathbf{\Pi}^T) \\ {\partial \mathbf{X} \over \partial \mathbf{d}} &= (-\mathbf{\Pi}^T\mathbf{X}_1 - \pi){1 \over \lambda}(\mathbf{I} - {1\over\lambda}\mathbf{d}\mathbf{\Pi}^T) \end{align}\]

计算对 \(\mathbf{\Pi}\) 的导数。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{\Pi}} &= -{\partial \mathbf{\Pi}^T\mathbf{X}_1{\mathbf{d} \over \lambda}\over \partial \mathbf{\Pi}} - {\partial {\pi \mathbf{d} \over \mathbf{d}^T\mathbf{\Pi}}\over \partial \mathbf{\Pi}} \end{align}\]

后一项为。

\[\begin{align} {\partial {\pi \mathbf{d} \over \mathbf{d}^T\mathbf{\Pi}}\over \partial \mathbf{\Pi}}&=-{\pi\over \lambda^2}\mathbf{d}\mathbf{d}^T \end{align}\]

前一项每个 element 分别计算。

\[\begin{align} \mathbf{\Pi}^T\mathbf{X}_1{\mathbf{d} \over \lambda} &= {X_1\Pi_1 + Y_1\Pi_2 + Z_1\Pi_3 \over d_1\Pi_1 + d_2\Pi_2 + d_3\Pi_3}\mathbf{d} \end{align}\]

令 \(\eta=\mathbf{\Pi}^T\mathbf{X}_1\)。

\[\begin{align} {\partial {\eta \over \lambda}\over \partial \Pi_1} &= {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) \\ {\partial {\eta \over \lambda}\over \partial \Pi_2} &= {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) \\ {\partial {\eta \over \lambda}\over \partial \Pi_3} &= {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) \\ \end{align}\]

\[\begin{align} {\partial \mathbf{\Pi}^T\mathbf{X}_1{\mathbf{d} \over \lambda}\over \partial \mathbf{\Pi}} &= \begin{bmatrix} {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_1 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_1 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_1 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_2 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_2 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_2 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_3 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_3 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_3 \end{bmatrix} \end{align}\]

所以。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{\Pi}} &= -\begin{bmatrix} {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_1 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_1 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_1 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_2 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_2 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_2 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_3 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_3 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_3 \end{bmatrix} + {\pi\over \lambda^2}\mathbf{d}\mathbf{d}^T \end{align}\]

综合以上。

\[\begin{align} {\partial \mathbf{X} \over \partial \mathbf{X}_1} &= \mathbf{I} - {\mathbf{d} \mathbf{\Pi}^T \over \lambda} \\ {\partial \mathbf{X} \over \partial \mathbf{d}} &= (-\eta - \pi){1 \over \lambda}(\mathbf{I} - {1\over\lambda}\text{diag}(d_1\Pi_1, d_2\Pi_2, d_3\Pi_3)) \\ {\partial \mathbf{X} \over \partial \mathbf{\Pi}} &= -\begin{bmatrix} {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_1 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_1 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_1 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_2 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_2 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_2 \\ {1 \over \lambda}(X_1 - {\eta \over \lambda}d_1) d_3 & {1 \over \lambda}(Y_1 - {\eta \over \lambda}d_2) d_3 & {1 \over \lambda}(Z_1 - {\eta \over \lambda}d_3) d_3 \end{bmatrix} + {\pi\over \lambda^2}\mathbf{d}\mathbf{d}^T \\ {\partial \mathbf{X} \over \partial \pi} &= - {\mathbf{d} \over \lambda} \\ \end{align}\]

3. 参数化

以上方向向量 \(\mathbf{d}\) 用 3 个数字表达，但是仅有 2 个自由度。所以需要做参数化，将对 3 维的导数转换为对 2 维的导数，随时计算一个切面即可。

平面方程 \(\mathbf{\pi}\) 也需要做参数化。注意到平面方程的最后一个数字是 \(-\mathbf{n}^T \mathbf{X}_0\)，即原点到平面距离，如果能确认平面与原点的距离足够大，不会接近 0，可以直接将最后一个数字设置为 1，其他数字自由优化。

不对平面做限制。平面方程 \(\mathbf{\pi}\) 用 4 个数字表达，实际有 3 个自由度。具体这 3 个自由度是什么，不重要。重要的是有 3 个自由度。如果能套用三维方向向量的参数化方式，那是极好的，但是三维向量的参数化方式涉及到计算叉乘，叉乘在四维空间中不存在，参考 [2]。从 4 维到 3 维，与四元数的维度一致，可以参考四元数的参数化方式，参考 [3] (18)。四元数的 global to local 的 4x3 矩阵的每一列都是四元数的零空间，也是平面方程的零空间。所以找到了 global to local 方法。

对于 plus 方法，即，计算得到的零空间三个方向的增量，将这三个增量乘以三个方向加到平面方程上，最后对平面方程做归一化，保证平面方程的模为 1。

Reference

[1] Hartley, Richard, and Andrew Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2003.

[2] Do four dimensional vectors have a cross product property? [duplicate] https://math.stackexchange.com/questions/720813/do-four-dimensional-vectors-have-a-cross-product-property.

[3] Sola, Joan. "Quaternion kinematics for the error-state KF." Laboratoire dAnalyse et dArchitecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep (2012).