使用2D相机进行3D位置估计

#1 楼

如果对象具有6个已知点（已知3D坐标，$ X，Y $和$ Z $），则可以计算与对象坐标系相关的摄影机位置。首先，一些基本知识。

同质坐标是欧几里得坐标$（X，Y，Z）$的矢量表示，我们在其中附加了所谓的比例因子$ \ omega $，使得同质坐标为$ \ textbf {X} = \ omega \ begin {bmatrix} X＆Y＆Z＆1 \ end {bmatrix} ^ T $。在您自己的计算中，尝试尽可能地使$ \ omega = 1 $保持不变（这意味着您可以通过将均匀坐标除以最后一个元素来对其进行“归一化”：$ \ textbf {X} \ leftarrow \ frac {\ textbf {X }} {\ omega} $）。我们还可以对2D点使用同质表示，例如$ \ textbf {x} = \ omega \ begin {bmatrix} X＆Y＆1 \ end {bmatrix} $（请注意，这些$ \ omega，X，Y $和$ Z $对于每个点都是不同的（2D或3D点）。均匀的坐标表示使数学运算更容易。

相机矩阵是从3D世界到图像传感器的$ 3 \ times4 $投影矩阵：

$$
\ textbf {x} = P \ textbf {X}
$$

其中$ \ textbf {x} $是图像传感器上的点（以像素为单位），而$ \ textbf {X} $是投影的3D点（假设它以毫米为单位）。

我们记得两个3向量之间的叉积可以定义为矩阵向量乘法，这样：

$$
\ textbf {v} \ times \ textbf {u} = \\
（\ textbf {v}）_x \ textbf {u} = \\
\开始{bmatrix}
0＆-v_3＆v_2 \\
v_3＆0＆-v_1 \\
-v_2＆v_1＆0
\ end {bmatrix}
\ textbf {u}
$$$

还要注意交叉生产$ \ textbf {v} \ times \ textbf {v} = \ textbf {0} $。

现在，让我们尝试根据前面的方程式求解投影矩阵$ P $。让我们将左侧的投影方程式与$ \ textbf {x} $ s叉积矩阵相乘：

$$
（\ textbf {x}）_ x \ textbf {x} =（\ textbf {x}）_ xP \ textbf {X} = \ textbf {0}
$$

啊哈！结果必须是零向量。如果现在打开等式，将得到：

$$
\ begin {bmatrix}
0＆-w＆y \\
w＆0＆-x \\
-y＆x＆0
\ end {bmatrix}
\ begin {bmatrix}
P_ {1,1}和P_ {1,2}和P_ {1,3}和P_ {1,4} \\
P_ {2,1}＆P_ {2,2}＆P_ {2,3}＆P_ {2,4} \\
P_ {3,1}＆P_ {3,2}＆P_ {3,3}＆P_ {3,4}
\ end {bmatrix}
\ textbf {X}
\\
=

\开始{bmatrix}
P_ {3,4} W y-P_ {2,1} X w-P_ {2,2} Y w-P_ {2,4} W w + P_ {3,1} X y-P_ {2,3} Z w + P_ {3,2} Y y + P_ {3,3} Z y \\
P_ {1,4} W w + P_ {1,1} X w-P_ {3,4} W x + P_ {1,2} Y w-P_ {3,1} X x + P_ {1,3} Z w-P_ {3,2} Y x-P_ {3,3} Z x \\
P_ {2,4} W x + P_ {2,1} X x-P_ {1,4} W y-P_ {1,1} X y + P_ {2,2} Y x-P_ {1,2} Y y + P_ {2,3} Z x-P_ {1,3} Z y
\ end {bmatrix} = \ textbf {0}
$$

只需少量重构，我们就可以在矩阵之外获得投影矩阵$ P $：

$$
\ tiny
\ begin {bmatrix} 0＆0＆0＆0＆-X \，w＆-Y \，w＆-Z \，w＆-W \，w＆X \，y＆Y \，y ＆Z \，y＆W \，y \\ X \，w＆Y \，w＆Z \，w＆W \，w＆0＆0＆0＆0＆-X \，x＆-Y \， x＆-Z \，x＆-W \，x \\-X \，y＆-Y \，y＆-Z \，y＆-W \，y＆X \，x＆Y \，x＆Z \，x＆W \，x＆0＆0＆0＆0 \ end {bmatrix}
\开始{bmatrix}
\ textbf {P} _1 \\
\ textbf {P} _2 \\
\ textbf {P} _3 \\
\ end {bmatrix}
=
\ textbf {0}
$$

其中$ \ textbf {P} _n $是$ n $的转置：相机矩阵$ P $的第n行。前一个（大）矩阵方程的最后一行是前两行的线性组合，因此不会带来任何其他信息，因此可以忽略。

小暂停，以便我们收集困难。请注意，必须为每个已知的3D-> 2D对应关系形成前一个矩阵方程（必须至少包含6个）。

现在，对于每个点对应关系，计算的前两行在上面的矩阵中，将$ 2 \ times12 $矩阵彼此堆叠，您将得到新的矩阵$ A $，其中

$$
A \ begin {bmatrix}
\ textbf {P} _1 \\
\ textbf {P} _2 \\
\ textbf {P} _3 \\
\ end {bmatrix}
=
\ textbf {0}
$$$

由于我们有12个未知数和（至少）12个方程式，因此可以解决。唯一的问题是，我们不想得到琐碎的答案，而
$$
\开始{bmatrix}
\ textbf {P} _1 \\
\ textbf {P} _2 \\
\ textbf {P} _3 \\
\ end {bmatrix} = \ textbf {0}
$$

幸运的是，我们可以使用奇异值分解（SVD）强制

$$
\ |
\开始{bmatrix}
\ textbf {P} _1 \\
\ textbf {P} _2 \\
\ textbf {P} _3 \\
\ end {bmatrix}
\ | = 1 因此，要求解方程式，请计算矩阵$ A $的SVD并选择与最小特征值对应的奇异矢量。该向量是矩阵A的零向量，也是相机矩阵$ P $的解。只需将$ \ begin {bmatrix} \ textbf {P} _1＆\ textbf {P} _2＆\ textbf {P} _3 \ end {bmatrix} ^ T $拆栈并形成$ P $。

现在您想知道到物体的距离。 $ P $定义为：

$$
P = K \开始{bmatrix} R和-R \ textbf {C} \ end {bmatrix}
$$

其中$ \ textbf {C} $是相对于对象原点的摄影机位置。可以通过计算$ P $ s的空向量从$ P $中求解。

（Hartley，Zisserman-计算机视觉中的多视图几何）

最后，当您计算两帧的摄像机位置时，您可以计算未知对象的位置（或对象某些点的位置） ）通过求解$ X $的两个方程式：

$$
\ textbf {x} _1 = P_1 \ textbf {X} \\
\ textbf {x} _2 = P_2 \ textbf {X} \\
$$

与解决相机矩阵的方式几乎相同：
$$
（\ textbf {x} _1）_xP_1 \ textbf {X} = \ textbf {0} \\
（\ textbf {x} _2）_xP_2 \ textbf {X} = \ textbf {0} \\
$$$

等等。