如何将图片上的像素坐标(u, v)投影到世界坐标系中, 即得到 \((x_w, y_w, z_w)\). 数学表达如下
\[\begin{align*} s\begin{bmatrix} u\\v\\1 \end{bmatrix} &= \begin{bmatrix} f_x & 0 & c_x & 0 \\ 0 & f_y & c_y & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} r_{00} & r_{01} & r_{02} & t_0 \\ r_{10} & r_{11} & r_{12} & t_1 \\ r_{20} & r_{21} & r_{22} & t_2 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_w\\y_w\\z_w \\1 \end{bmatrix} \end{align*} \]通过两个方程求解3个未知数具有无穷个解, 不过我们的问题具有特殊性, 即我们的世界坐标系的 \(xoy\) 平面建立在路面上, 而我们已知车的3D模型, 那么在假设关键点预测和路面方程完全没有误差的情况下, 我们是知道图片中车关键点在世界坐标系下的高度的(即已知 \(z_w\)), 这样问题就转化为解答二元二次方程, 唯一解.
解如下
xw ((cx*r21 + fx*r01 - r21*u)*(cy*r22*zw + cy*t3 + fy*r12*zw + fy*t2 - r22*v*zw - t3*v) - (cy*r21 + fy*r11 - r21*v)*(cx*r22*zw + cx*t3 + fx*r02*zw + fx*t1 - r22*u*zw - t3*u))/((cx*r20 + fx*r00 - r20*u)*(cy*r21 + fy*r11 - r21*v) - (cx*r21 + fx*r01 - r21*u)*(cy*r20 + fy*r10 - r20*v))
yw (-(cx*r20 + fx*r00 - r20*u)*(cy*r22*zw + cy*t3 + fy*r12*zw + fy*t2 - r22*v*zw - t3*v) + (cy*r20 + fy*r10 - r20*v)*(cx*r22*zw + cx*t3 + fx*r02*zw + fx*t1 - r22*u*zw - t3*u))/((cx*r20 + fx*r00 - r20*u)*(cy*r21 + fy*r11 - r21*v) - (cx*r21 + fx*r01 - r21*u)*(cy*r20 + fy*r10 - r20*v))