3D reconstruction from a 2D image in MATLAB - image

Question

I have some difficulties to reconstruct a 3D scene from a 2D image under Matlab.
I would like to create a top view of scene removing the perspective, in other words, realize an inverse perspective mapping.
Let's assume we know the camera position, orientation and its parameters. Moreover we consider all the captured points lie on the same plane XY.
Then, it is easy to prove that a pixel at a (u,v) location in the image will move to the coordinate (X,Y,0) in the 3D space with:
X=-((u*P(3,4)-P(1,4))*(v*P(3,1)-P(2,1)) + (v*P(3,4)-P(2,4))*(P(1,1)-u*P(3,1)))/((u*P(3,2)-P(1,2))*(v*P(3,1)-P(2,1)) + (v*P(3,2)-P(2,2))*(P(1,1)-u*P(3,1)));
Y=(X*(u*P(3,2)-P(1,2)) + (u*P(3,4)-P(1,4)))/(P(1,1)-u*P(3,1));
P is the projection matrix such that: P=[KR KT] with K,R and T respectively the intrinsic, rotation and translation matrices.
Once all the 3D locations of each pixel are computed, I would like to display the XY plane with the color information of the original pixel as if it was a 2D image.
However, a pixel (u,v) can mapped in 3D space to a non integer location meaning that I get a non-regular scatter plot were each (X,Y) point contain a color information.
I tried to divide the XY plane into small windows and then compute the average color of all points into each squares but it is very slow.
Please find my code below.
Some help would be appreciated.
Thank you in advance,
Pm
% This program aims to convert a 2D image into a 3D scenario. Let's assume that
% all the points in the image lie on the same plan in the 3D space.
% Program:
% 1-Generate the 3D scenario with 4 squares from diferrent colors
% 2-Take a picture of these squares by projecting the scene into an image 2D space
% 3-Reconstruct the 3D scene from the 2D picture
% Author: Pierre-Marie Damon
clear all; close all; clc;
%% 4 SQUARES DEFINITION
c=10;
sq1_3D = [ 0 0 0;
0 c 0;
c c 0;
c 0 0];
sq2_3D = [ 0 0 0;
c 0 0;
c -c 0;
0 -c 0];
sq3_3D = [ 0 0 0;
0 -c 0;
-c -c 0;
-c 0 0];
sq4_3D = [ 0 0 0;
-c 0 0;
-c c 0;
0 c 0];
SQ_3D = [sq1_3D;
sq2_3D;
sq3_3D;
sq4_3D];
%% CAMERA DEFINITION
% Image resolution:
image_size = [640,480];
% Intrinsic matrix:
fx=150; fy=150; % fx, fy: focal lengths in pixel
x0=image_size(1)/2; y0=image_size(2)/2; % x0, y0: optical center projection coordinates in the 2D cxamera space
K = [fx 0 x0 ;
0 fy y0 ;
0 0 1 ];
% 3D camera orientation:
Rot_cam = [ 0 0 1;
-1 0 0;
0 -1 0]';
% 3D camera rotations:
yaw = -20*pi/180;
roll = -0*pi/180;
pitch = -20*pi/180;
% 3D camera position:
O_Rcam = [-20;0;10];
h_cam = O_Rcam(3); % camera height
% Projection & transformation matrices
R = rotationVectorToMatrix([pitch,yaw,roll])*Rot_cam; % Rotation matrix
T = -R*O_Rcam; % Translation matrix
P = [K*R K*T; zeros(1,3) 1]; %Projection Matrix
%% PROJECTION FROM 3D TO THE 2D IMAGE
SQ_2D = (P*[SQ_3D ones(size(SQ_3D,1),1)]')'; % homogeneous coordinates
SQ_2D = SQ_2D(:,1:2)./repmat(SQ_2D(:,3),1,2); % Normalization
% Square splits:
sq1_2D = SQ_2D(1:4,:);
sq2_2D = SQ_2D(5:8,:);
sq3_2D = SQ_2D(9:12,:);
sq4_2D = SQ_2D(13:16,:);
%% PLOT THE 3D SCENARIO
figure('units','normalized','outerposition',[0 0 1 1]);
f1=subplot(1,2,1);hold on; grid on; view(50, 30); axis equal;
xlabel('X');ylabel('Y');zlabel('Z'); title('3D Scene');
% Plot the camera
cam = plotCamera('Location',O_Rcam,'Orientation',R,'Size',1,'AxesVisible',0);
% Plot the squares
patch([sq1_3D(:,1)],[sq1_3D(:,2)],'r','EdgeColor','k');
patch([sq2_3D(:,1)],[sq2_3D(:,2)],'g','EdgeColor','k');
patch([sq3_3D(:,1)],[sq3_3D(:,2)],'b','EdgeColor','k');
patch([sq4_3D(:,1)],[sq4_3D(:,2)],'m','EdgeColor','k');
%% PLOT THE 2D IMAGE
f2=subplot(1,2,2); hold on; grid on; axis equal; set(gca,'YDir','Reverse'); title('2D Image'); axis off;
xlim([0 image_size(1)]); ylim([0 image_size(2)]);
% plot the projected squares
patch ([sq1_2D(:,1)],[sq1_2D(:,2)],'r','EdgeColor','none');
patch ([sq2_2D(:,1)],[sq2_2D(:,2)],'g','EdgeColor','none');
patch ([sq3_2D(:,1)],[sq3_2D(:,2)],'b','EdgeColor','none');
patch ([sq4_2D(:,1)],[sq4_2D(:,2)],'m','EdgeColor','none');
% Plot the image borders
plot([0 image_size(1)],[0 0],'k','linewidth',3);
plot([image_size(1) image_size(1)],[0 image_size(2)],'k','linewidth',3);
plot([0 image_size(1)],[image_size(2) image_size(2)],'k','linewidth',3);
plot([0 0],[0 image_size(2)],'k','linewidth',3);
%% GENERATE A JPG IMAGE
figure; hold on; grid on; set(gca,'YDir','Reverse'); axis off;
hFig = gcf; hAx = gca; % get the figure and axes handles
set(hFig,'units','normalized','outerposition',[0 0 1 1]); % set the figure to full screen
set(hAx,'Unit','normalized','Position',[0 0 1 1]); % set the axes to full screen
set(hFig,'menubar','none'); % hide the toolbar
set(hFig,'NumberTitle','off'); % to hide the title
xlim([0 image_size(1)]); ylim([0 image_size(2)]);
% Plot the squares
patch ([sq1_2D(:,1)],[sq1_2D(:,2)],'r','EdgeColor','none');
patch ([sq2_2D(:,1)],[sq2_2D(:,2)],'g','EdgeColor','none');
patch ([sq3_2D(:,1)],[sq3_2D(:,2)],'b','EdgeColor','none');
patch ([sq4_2D(:,1)],[sq4_2D(:,2)],'m','EdgeColor','none');
% Create the image
set(gcf,'PaperUnits','inches','PaperPosition',[0 0 image_size(1)/100 image_size(2)/100])
print -djpeg Image.jpg -r100
save('ImageParam.mat', 'K','R','T','P','O_Rcam' )
%% 3D RECONSTRUCTION FROM 2D IMAGE
clear all;
close all;
clc;
load('ImageParam');
I=imread('Image.jpg');
I1 = rgb2gray(I);
figure;imshow(I1);impixelinfo;
I2 = zeros(size(I1));
k=1; i=1; j=1;
tic
for y=size(I2,1):-1:1
for x=1:size(I2,2)
% The formula below comes from the projection equations of
% the camera and the additional constraint that all the points lie on the XY
% plane
X(k)=-((x*P(3,4)-P(1,4))*(y*P(3,1)-P(2,1)) + (y*P(3,4)-P(2,4))*(P(1,1)-x*P(3,1)))/((x*P(3,2)-P(1,2))*(y*P(3,1)-P(2,1)) + (y*P(3,2)-P(2,2))*(P(1,1)-x*P(3,1)));
Y(k)=(X(k)*(x*P(3,2)-P(1,2)) + (x*P(3,4)-P(1,4)))/(P(1,1)-x*P(3,1));
Z(k)=0;
C(k)=I1(y,x); % Color (gray intensity) information
k=k+1;
end
end
toc
figure;hold on;axis equal;
plot(X,Y,'.');
grid on;