Location in Space

Posted on Edited on In Waline:

本文是本人对空间中点的位置的推断的一些想法，有可能其他人已经有了类似的想法，权当做笔记了。

空间中位置的推断

1 目的

  • 在空间中的某一位置，有一个物体 ObjectObject 出现，在地面上有至少三台设备利用相机拍摄到了这个物体。
  • 假设 ObjectObject 的真实坐标为 Qgt=(xgt,ygt,zgt)Q^{gt} = (x^{gt},y^{gt},z^{gt})。
  • 假设设备编号为 i=1,2,...,Ni=1,2,...,N，设备 ii 的拍摄坐标为 ξi=(xi,yi,zi)⊤\xi_i=(x_i,y_i,z_i)^\top，仰角为 ωi=(αi,βi,γi)⊤\omega_i=(\alpha_i,\beta_i,\gamma_i)^\top。
  • 实际中，可以记 ξi=(longitudei,latitudei,altitudei)⊤\xi_i=(longitude_i,latitude_i,altitude_i)^\top，而 αi, βi, γi\alpha_i,\ \beta_i,\ \gamma_i 表示拍摄方向的方向向量与 xOy, yOz, xOzxOy,\ yOz,\ xOz 平面的夹角。

2 推导过程

2.1 由 α, β, γ\alpha,\ \beta,\ \gamma 推导拍摄点 ii 拍摄方向的方向向量

设 ii 点的方向向量为 di=(di(x),di(y),di(z))⊤, ∣∣di∣∣=1d_i = (d^{(x)}_i, d^{(y)}_i, d^{(z)}_i)^\top,\ ||d_i|| = 1，则

tan⁡α=dzdx2+dy2, tan⁡β=dxdy2+dz2, tan⁡γ=dydx2+dz2\tan \alpha = \frac{d_z}{\sqrt{d_x^2 + d_y^2}},\ \tan \beta = \frac{d_x}{\sqrt{d_y^2 + d_z^2}},\ \tan \gamma = \frac{d_y}{\sqrt{d_x^2 + d_z^2}}

或

sin⁡α=dz∣∣d∣∣=dz, sin⁡β=dx, sin⁡γ=dy\sin \alpha = \frac{d_z}{||d||} = d_z,\ \sin \beta = d_x,\ \sin \gamma = d_y

从而有

di=(sin⁡αi,sin⁡βi,sin⁡γi)⊤d_i = (\sin \alpha_i,\sin \beta_i,\sin \gamma_i)^\top

2.2 由 nin_i 和 ξi\xi_i 推导出点 ii 的拍摄方向所在直线 lil_i 方程

由设定可得，lil_i 经过拍摄点 ξi\xi_i，因此

li: x−xisin⁡αi=y−yisin⁡βi=z−zisin⁡γil_i:\ \frac{x - x_i}{\sin \alpha_i} = \frac{y - y_i}{\sin \beta_i} = \frac{z - z_i}{\sin \gamma_i}

2.3 寻找直线 lil_i 和 ljl_j 上的点 PiP_i 和 PjP_j 使得 min⁡∣∣Pi−Pj∣∣2\min ||P_i - P_j||^2

理想情况下，为找到 ObjectObject 的坐标，只需找到直线 lil_i 和 ljl_j 的交点。

然而，由于拍摄角度的问题，无法保证 lil_i 和 ljl_j 一定相交，因此先找到点 PiP_i 和 PjP_j 使 min⁡∣∣Pi−Pj∣∣2\min ||P_i - P_j||^2 即两点间距离最短。

首先，lil_i 和 ljl_j 必过点 Ai=ξiA_i = \xi_i 和 Aj=ξjA_j = \xi_j，则有 OAi→=ξi\overrightarrow{OA_i} = \xi_i，OAj→=ξj\overrightarrow{OA_j} = \xi_j，再由向量加法，有 OPi→=OAi→+AiPi→\overrightarrow{OP_i} = \overrightarrow{OA_i} + \overrightarrow{A_iP_i}，又由于 AiPi→\overrightarrow{A_iP_i} 与方向向量 did_i 共线，得

OPi→=ξi+ti⋅di, (ti∈R)\overrightarrow{OP_i} = \xi_i + t_i \cdot d_i,\ (t_i \in R)

同理

OPj→=ξj+tj⋅dj, (tj∈R)\overrightarrow{OP_j} = \xi_j + t_j \cdot d_j,\ (t_j \in R)

点 PiP_i 和 PjP_j 间的距离为

∣PiPj→∣=∣OPj→−OPi→∣=∣(ξj−ξi)+tj⋅dj−ti⋅di∣|\overrightarrow{P_i P_j}| = |\overrightarrow{OP_j} - \overrightarrow{OP_i}| = |(\xi_j - \xi_i) + t_j \cdot d_j - t_i \cdot d_i|

两边取平方，得到关于 tit_i 和 tjt_j 的函数，求最小值

y=[(ξj−ξi)+tj⋅dj−ti⋅di]2=ti2∣di∣2−2titj(di⋅dj)+tj2∣dj∣2−2ti[(ξj−ξi)⋅di]+2tj[(ξj−ξi)⋅dj]+(ξj−ξi)2\begin{aligned} y &= [(\xi_j - \xi_i) + t_j \cdot d_j - t_i \cdot d_i]^2 \\ &= t_i^2 |d_i|^2 - 2t_i t_j (d_i \cdot d_j) + t_j^2 |d_j|^2 - 2 t_i [(\xi_j - \xi_i) \cdot d_i] + 2 t_j [(\xi_j - \xi_i) \cdot d_j] + (\xi_j - \xi_i)^2 \end{aligned}

求解上述方程最小值，解得 tit_i 和 tjt_j，即可求得 OPi→\overrightarrow{OP_i} 和 OPj→\overrightarrow{OP_j}，即求出 PiP_i 和 PjP_j 的坐标。

也可以利用当 PiP_i 和 PjP_j 距离取最小值时，PiPj→\overrightarrow{P_i P_j} 与方向向量的垂直关系求解 tit_i 和 tjt_j：

{PiPj→⋅di=0PiPj→⋅dj=0\left\{ \begin{matrix} \overrightarrow{P_i P_j} \cdot d_i = 0 \\ \overrightarrow{P_i P_j} \cdot d_j = 0 \end{matrix} \right.

2.4 估计 ObjectObject 的坐标

由上一部分，可以求得两个点 PiP_i 和 PjP_j 使得它们之间的距离在拍摄直线 lil_i 和 ljl_j 间最短，由于目的都是拍摄到 ObjectObject，因此可以假设 ObjectObject 就在线段 PiPjP_iP_j 附近，也可以说再线段中点 PijP_{ij} 附近。

注意，前提是必须有三台设备拍摄 ObjectObject，因此有三条拍摄直线 lil_i，ljl_j，lkl_k，按照上面的计算过程，可以得到三个中点

Pij, Pjk, PikP_{ij},\ P_{jk},\ P_{ik}

（注意，上面表达中隐含有 Pij=PjiP_{ij} = P_{ji}）

那么 ObjectObject 即在这三个点附近，再取这三个点的中点，记其为 ObjectObject 的坐标估计值

O=Pij+Pjk+Pik3O = \frac{P_{ij} + P_{jk} + P_{ik}}{3}

当然，若有 NN 条拍摄直线，那么可以得到 CN2=N(N−1)2C_N^2 = \frac{N(N-1)}{2} 个中点，此时 ObjectObject 的坐标估计值可以写为

O=∑i=1N∑j=i+1NPijNO = \frac{\sum_{i=1}^N \sum_{j=i+1}^{N} P_{ij}}{N}

有了估计值，此问题还能化为最小二乘问题

min⁡f=∣∣Ogt−O∣∣2\min f = ||O^{gt} - O||^2