%these values are taken from running the Learning OpenCV example ch12_ex12_3.cpp

clear all;
clc; 

% P1 and P2 from stereoRectify
P1 = [450.68550    0.00000  322.03562    0.00000 
   0.00000  450.68550  241.23167    0.00000 
   0.00000    0.00000    1.00000    0.00000]

P2 = [450.68550    0.00000  322.03562 -1505.54347 
   0.00000  450.68550  241.23167    0.00000 
   0.00000    0.00000    1.00000    0.00000]

%P2(1,4) = f_x * T_x , so...
T_x = P2(1,4)/P2(1,1)

% a point in the camera's coordinate frame  x right, y down, Z forward
X = [1 2 5 1]'

% project left
x_homL = P1 * X;

% left image coordinate
xL(1) = x_homL(1) / x_homL(3);
xL(2) = x_homL(2) / x_homL(3);
xL
% right image coordinate
x_homR = P2 * X;

xR(1) = x_homR(1) / x_homR(3);
xR(2) = x_homR(2) / x_homR(3);
xR

% disparity (positive)
d = xL(1) - xR(1)

uvd1 = [xL'; d; 1]

% Q from stereoRectify
Q = [1.00000    0.00000    0.00000 -322.03561 
   0.00000    1.00000    0.00000 -241.23167 
   0.00000    0.00000    0.00000  450.68549 
   0.00000    0.00000   -0.29935   -0.00000]

%Q(4,3) is supposed to be -1/Tx, but actually is 1/Tx
% changing this gives the correct result for the reprojection
correct_value = -1/T_x

% reprojected X in homogeneous coords
reX_hom = Q * uvd1;

% reprojected X in 3D coordinates, result is behind camera
reX = reX_hom(1:3) / reX_hom(4)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Output
% P1 =
% 
%        450.69            0       322.04            0
%             0       450.69       241.23            0
%             0            0            1            0
% 
% 
% P2 =
% 
%        450.69            0       322.04      -1505.5
%             0       450.69       241.23            0
%             0            0            1            0
% 
% 
% T_x =
% 
%       -3.3406
% 
% 
% X =
% 
%      1
%      2
%      5
%      1
% 
% 
% xL =
% 
%        412.17       421.51
% 
% 
% xR =
% 
%        111.06       421.51
% 
% 
% d =
% 
%        301.11
% 
% 
% uvd1 =
% 
%        412.17
%        421.51
%        301.11
%             1
% 
% 
% Q =
% 
%             1            0            0      -322.04
%             0            1            0      -241.23
%             0            0            0       450.69
%             0            0     -0.29935            0
% 
% 
% correct_value =
% 
%       0.29935
% 
% 
% reX =
% 
%            -1
%            -2
%            -5