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

我有一个摄像头（iPhone），图像中有一个3D控制对象，我非常了解它的属性。（我的控制对象）。运动中还有第二个物体。最终目标是建立给定时间段内运动对象的3D轨迹。（跟踪）

我想问一下，我能找出答案吗？

• 手机到控制对象的距离（为便于讨论，我们假设摄像头处于一定高度，并且一定距离两者都不是已知的，但是摄像头垂直于已知的表面）

• 我可以在每个后续帧中找到对象的辅助对象。我的目标是如上所述评估其3D轨迹。

还有一个问题，我们可以将系统设置为可以设置手机到控制对象的距离（尽管不是首选），这对我的第二点有帮助吗？

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

首先是一些基础知识。

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

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

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

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

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

$$\textbf{v} \times \textbf{u} = \\ ( \textbf{v} )_x \textbf{u} = \\ \begin{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}$

$$(\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} \\ = \begin{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} \begin{bmatrix} \textbf{P}_1 \\ \textbf{P}_2 \\ \textbf{P}_3 \\ \end{bmatrix} = \textbf{0}$$

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

稍作停顿，以便我们集中精力。请注意，必须为每个已知的3D-> 2D对应关系形成前一个矩阵方程（必须至少有6个）。

现在，对于每一个点对应，计算上述矩阵的前两行，可堆叠在彼此的顶部矩阵，你会得到新的矩阵为其$2\times12$$A$

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

由于我们有12个未知数和（至少）12个方程式，因此可以解决。唯一的问题是我们不想得到简单的答案，其中

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

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

$$\| \begin{bmatrix} \textbf{P}_1 \\ \textbf{P}_2 \\ \textbf{P}_3 \\ \end{bmatrix} \|=1$$

因此，为了求解方程式，计算矩阵 SVD 并选择对应于最小特征值的奇异矢量。该向量是矩阵A的零向量，也是相机矩阵。只需堆叠并形成。$A$$P$$\begin{bmatrix} \textbf{P}_1 & \textbf{P}_2 & \textbf{P}_3 \end{bmatrix}^T$$P$

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

$$P = K\begin{bmatrix}R & -R\textbf{C}\end{bmatrix}$$

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

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

最后，当您计算两帧的摄像机位置时，可以通过求解两个方程来计算未知对象的位置（或对象某些点的位置）：$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} \\ $$

等等。