# recovery plane from 4 point imaging using calibration camera

I have a camera and its K matrix (calibration matrix) also I have image of plane, I know the real points of the 4 corners and thier correspondence pixel. I know how to compute the H matrix if z=0 (H is homography matrix between Image and the real plane). And Now I try to get the real point of the plane (3D point) with the rotation matrix and the transltion vector I follow this paper :Calibrating an Overhead Video Camera by Raul Rojas in section 3 - 3.3. My code is:

ImagePointsScreen=[16,8,1;505,55,1;505,248,1;44,301,1;];

RealPointsMirror=[0,0,1;9,0,1;9,6,1;0,6,1]; %Mirror
RealPointsScreen=[0,0,1;47.5,0,1;47.5,20,1;0,20,1];%Screen
imagesc(screenImage);
hold on
for i=1:4
drawBubble(ImagePointsScreen(i,1),ImagePointsScreen(i,2),1,'g',int2str(i),'r')
end

Points3DScreen=Get3DpointSurface(RealPointsScreen,ImagePointsScreen,'Screen');

figure
hold on
plot3(Points3DScreen(:,1),Points3DScreen(:,2),Points3DScreen(:,3));
for i=1:4
drawBubble(Points3DScreen(i,1),Points3DScreen(i,2),1,'g',int2str(i),'r')
end

function [ Points3D ] = Get3DpointSurface( RealPoints,ImagePoints,name)
M=zeros(8,9);

for i=1:4

M((i*2)-1,1:3)=-RealPoints(i,:);
M((i*2)-1,7:9)=RealPoints(i,:)*ImagePoints(i,1);
M(i*2,4:6)=-RealPoints(i,:);
M(i*2,7:9)=RealPoints(i,:)*ImagePoints(i,2);

end

[U S V] = svd(M);
X = V(:,end);
H(1,:)=X(1:3,1)';
H(2,:)=X(4:6,1)';
H(3,:)=X(7:9,1)';
K=[680.561906875074,0,360.536967117290;0,682.250270165388,249.568615725655;0,0,1;];

newRO=pinv(K)*H;
h1=newRO(1:3,1);
h2=newRO(1:3,2);

scaleFactor=(norm(h1)+norm(h2))/2;
newRO=newRO./scaleFactor;
r1=newRO(1:3,1);
r2=newRO(1:3,2);
r3=cross(r1,r2);
r3=r3/norm(r3);

R=[r1,r2,r3];

RInv=pinv(R);
O=-RInv*newRO(1:3,3);
M=K*[R,-R*O];
for i=1:4
res=pinv(M)* [ImagePoints(i,1),ImagePoints(i,2),1]';
res=res';
res=res*(1/res(1,4));
Points3D(i,:)=res';

end
Points3D(i+1,:)=Points3D(1,:);  %just add the first point to the end of the array for draw square

end


My problem is that I don't get good results

1.The point 1 is at (0,0,0) and this is not the real location

2.the points are upside down

What I am doing worng?

-