I have 2 images, called left and right images. I have some matched points $[c_l,r_l]$ and $[c_r,r_r]$ in both of them (these points are in pixel coordinates).
For a 3D point in the real world, they are related by:
$\lambda_l*[c_l , r_l,1]^t = W_l*[X,Y,Z]^t$
$\lambda_r*[c_r ,r_r,1]^t = W_r*[R|T]*[X,Y,Z,1]^t$
Here $W_l$ and $W_r$ are the intrinsic parameters of the left and right cameras and $T and R$ are the relative / overall translation and rotation between the cameras.
I attempted to solve this as:
$Q*[M,1]^T = 0$
where Q is
$$ \begin{matrix} (k_1 +k_2-k_3*c_r-k_3*r_r) & (tw_x + tw_y -r_r*tw_z - c_r*tw_z) \\ (w_1 + w_2 - w_3*c_l -w_3*r_l) & 0 \\ \end{matrix} $$
and M is $[X,Y,Z]$
Here:
- $k1,k2, k3$ are the rows of the matrix K given by $K = R_r*R$
- $tw$ is the vector formed by $tw = W_r*T$, its elements are $[tw_x,tw_y,tw_z]$
- $w1, w2, w3$ are the rows of the matrix $W_l$
I'm solving this as:
$Q*[M,1]^t = 0$
I look for a solution in the null space of $Q$.
$[M,1]^t$ would be hence be the null vector of Q.
That is,
$svd(Q) = [U,S,V]$ and $[M,1]^t$ would be the last column of V.
The result I get would be of form $[mx,my,mz,mk]$ so I simply divide all elements by $mk$ to get something of the form
$[mx/mk,my/mk,mz/mk,1]$. I consider this to be the value of X,Y,Z
Is there a better way to solve this? Correct me if I'm wrong here but I'm feel this could have been done in a simpler (more accurate) fashion.
