Calculate XYZ for point P in realworld for imagepoint O (with Instrinsics and Extrensics)

I want to calculate the XYZ cordination of point P with this equation:

u and v = location in pixels

M= IntrinsicMatrix.

R= RotationMatrices

t= translationvectors

XYZ= realworldpoint.

I already calculated the stereoParams (called cameraParams) and use the script below. My problem is that the XYZ i found are wrong. So my question is what do I wrong?

im1=imread(fullfile(imageDir, sprintf('imgL-01.bmp')));
im2=imread(fullfile(imageDir, sprintf('imgR-01.bmp')));
JL=undistortImage(im1,cameraParams.CameraParameters2);
JR=undistortImage(im2,cameraParams.CameraParameters1);

[imagePoints, boardSize] = detectCheckerboardPoints(JL);
[R1,t1] = extrinsics(imagePoints,worldPoints,cameraParams.CameraParameters2);
M1 = cameraParams.CameraParameters2.IntrinsicMatrix;

Fx=M1(1);
Cx=M1(3);
Fy=M1(5);
Cy=M1(6);
lambda = Cx/(Fx^2)+Cy/(Fy^2)+1-(Cy*Cy/(Fx^2*Fy^2)+Cx^2/(Fx^4))/Fx^2;
s1=1/lambda;

P1 = M1'*[R1',t1']/s1;
A=[imagePoints(16,1);imagePoints(16,2);1];
PX1 =  [P1(1), P1(4), P1(7), -P1(10)+A(1);
        P1(2), P1(5), P1(8), -P1(11)+A(2);
        P1(3), P1(6), P1(9), -P1(12)+A(3)];
[~,Q1]=GaussJordanElimination(PX1);    

[imagePoints, boardSize] = detectCheckerboardPoints(JR);
[R2,t2] = extrinsics(imagePoints,worldPoints,cameraParams.CameraParameters1);
M2 = cameraParams.CameraParameters1.IntrinsicMatrix;

Fx=M2(1);
Cx=M2(3);
Fy=M2(5);
Cy=M2(6);
lambda = Cx/(Fx^2)+Cy/(Fy^2)+1-(Cy*Cy/(Fx^2*Fy^2)+Cx^2/(Fx^4))/Fx^2;
s2=1/lambda;

A=[imagePoints(16,1);imagePoints(16,2);1];
P2 = M2'*[R2',t2']/s2
PX2 =  [P2(1), P2(4), P2(7), -P2(10)+A(1);
        P2(2), P2(5), P2(8), -P2(11)+A(2);
        P2(3), P2(6), P2(9), -P2(12)+A(3)];
[~,Q2]=GaussJordanElimination(PX2);  

RC = cameraParams.RotationOfCamera2;
tC = [cameraParams.TranslationOfCamera2(1);...
        cameraParams.TranslationOfCamera2(2);...
        cameraParams.TranslationOfCamera2(3)];

Q2=[Q2(:,1:3),RC*Q2(:,4)]+[[0 0 0; 0 0 0; 0 0 0],tC];

disp(Q1)
disp(Q2)

PS: I have matlab 2014a (student), so I have no upgrades rights and can not use any matlab 2014b functions like triangulate or camMatrix.

Answer 1

[ITACHI UCHIHA]

sorry to say but your mathematics is little wrong here, sir, you have used homogeneous 3d coordinates system in right hand side [x;y;z;1] and on left hand side of the equation you have used homogeneous 2d system; either use S[u;v;0;1]=M[R,T][X;Y;Z;1];But since your image is always going to be a 2d image means Z is going to be zero in every case .So better use homogeneous 2d coordinate system i.e.,S[u;v;1]=M[R,T][X;Y;1];