Line-Plane 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 的 4×3 矩阵的每一列都是四元数的零空间，也是平面方程的零空间。所以找到了 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).