The answer:
- Undistort images.
cv::undistort
- calculate
P1ccdf = A^(-1)*P1
-> P1 point in f=1 world ref coords. - Calculate the point in the world ref
Z=0
for that:- Calculate the optical centre coords in the world: Copt1=[0,0,0,1]T
Copt1w=-R1t*T1
- compute the vector between Coptw1 and P1ccdf: with some math ->
V1w=R1T*P1ccdf
Coptw1+lambda*V1w=[Pxw,Pyw,0]T
->lambda=-cpotw(z)/V1(z)
Coptw1+lambda*V1w=[Pxw,Pyw,0]T=P1w
- Calculate the optical centre coords in the world: Copt1=[0,0,0,1]T
- Protect world point into cam2
P1ccd2f = R2T2 *[Pxw Pyw 0 1]T
P1ccd2f=P1ccd2f/P1ccd2f(3)
P2=A2*P1ccd2f
where
P
means pointA
is the intrinsic matrix (4x4)RT
is the camera matrix (3x4)