function [total,omega,R,T,f,c,k,ex,Horizon,distance] = auto_calib_cam(I,map,center,one_f,clickname);

%function [omega,R,T,f,c,k,ex] = AutoCalibCam(I,map);
%
%Function to calibratre the camera. Everything is done automatically.
%Set center to 1 to enforce minimization over the camera center c
%Otherwise, c is kept in the middle of the image.
%
% California Institute of Technology
% (c) Jean-Yves Bouguet and Markus Weber

% updated Feb 1st, 1997 for the new larger calibration grid
%% Slighly different values for dX and dY...

% updated March 25th, 1997 for the unique focal length quantity
% Plus, save the clicked corners under a file
     

if (nargin < 1),
   error('Are you sure you want to do calibration without an image?');
end;

wintx = 5; % neigborhood of integration for
winty = 5; % the corner finder

clickname = [];

if nargin < 2, 
   map = gray(256);
end;

[ny,nx] = size(I);

figure(2);
image(I);
colormap(map);

if isempty(clickname); % == [];

   %title('Click on the four corners of the pattern...');
   
   disp('Click on the four corners of the complete pattern...');
   
   [x,y] = ginput2(4);
   
else
   eval(['load ' clickname]); % load the 4 corners clicked
end;


%load corners;
%sil=2; 
%sil2 = silvio(sil);


Xc = corner_finder([x';y'],I,wintx,winty); % the four corners

x = Xc(1,:)';
y = Xc(2,:)';

[y,indy] = sort(y);
x = x(indy);

if (x(2) > x(1)),
   x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2);
else
   x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1);
end;
if (x(3) > x(4)),
   x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4);
else
   x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3);
end;

x = [x1;x2;x3;x4];
y = [y1;y2;y3;y4];


figure(2); hold on;
plot(x,y,'r+');
hold off;

%save clicked_corners x y 

%keyboard;

n_sq_x = input('Number of squares per row [9] = '); %6
if isempty(n_sq_x), n_sq_x = 9; end;
n_sq_y = input('Number of squares per column [10] = '); %6
if isempty(n_sq_y), n_sq_y = 10; end;


%dX = input('Size dX in x of the squares [1.62cm] = ');
%dY = input('Size dY in y of the squares  [1.6cm] = ');
%if isempty(dX), dX = 1.62; end;
%if isempty(dY), dY =  1.6; end;

 dX = input('Size dX in x of the squares [1.9912cm] = ');
 dY = input('Size dY in y of the squares  [2.0139cm] = ');
 if isempty(dX), dX = 21.9250/11; end;
 if isempty(dY), dY =  18.1250/9; end;




[XX] = projected_grid([x(1);y(1)],[x(2);y(2)],[x(3);y(3)],[x(4);y(4)],n_sq_x+1,n_sq_y+1); % The full grid

figure(2);
hold on;
zoom on;
plot(XX(1,:),XX(2,:),'r+');
title('The red crosses should be on the corners...');
hold off;

W = n_sq_x*dX;
L = n_sq_y*dY;


quest_distort = [];
%input('Use an initial guess for distortion? ([]=no, other=yes) ');
   
quest_distort = ~isempty(quest_distort);
   
if quest_distort,
   
   % Estimation of focal length:
   c_g = [size(I,2);size(I,1)]/2 + .5;
   
   f_g = distor_2_calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1);
   
   f_g = mean(f_g);
   
   script_fit_distortion;
   
end;
   
%figure(2); hold on;
%plot(XX(1,:),XX(2,:),'c+');
%hold off;

Np = (n_sq_x+1)*(n_sq_y+1);

disp('Corner extraction...');

grid_pts = corner_finder(XX,I,wintx,winty); 		%%% Finds the exact corners at every points!

%save all_corners x y grid_pts

grid_pts = grid_pts - 1;

%grid_pts = XX; % no corner finder

ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners
ind_orig = (n_sq_x+1)*n_sq_y + 1;
xorig = grid_pts(1,ind_orig);
yorig = grid_pts(2,ind_orig);
dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]');
dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]');


figure(3);
image(I); colormap(map); hold on;
plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+');
plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo');
plot(xorig+1,yorig+1,'*m');
h = text(xorig-15,yorig-15,'O');
set(h,'Color','m','FontSize',14);
h2 = text(dxpos(1)-10,dxpos(2)-10,'dX');
set(h2,'Color','g','FontSize',14);
h3 = text(dypos(1)-25,dypos(2)-3,'dY');
set(h3,'Color','g','FontSize',14);
xlabel('Xc (in camera frame)');
ylabel('Yc (in camera frame)');
title('Extracted corners');
zoom on;
drawnow;
hold off;


%dX = 19.9/10; 				%%% Size along x
%dY = 16.1/8; 				%%% Size along y


Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)';
Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)';
Zi = zeros(1,Np);

Xgrid = [Xi;Yi;Zi];

Xgrid_2 = Xgrid - mean(Xgrid')'*ones(1,Np);


%%%% Explicit Calibration:

c = [nx/2 ny/2]' - 0.5;

grid_pts_centered = [grid_pts(1,:) - c(1);grid_pts(2,:) - c(2)];


satis_global = 0;

while ~satis_global,


%ques_focal = input('Use two ([]) or one (other) focals? ');
ques_focal = 2;  %% selected 'other' by default

if ~isempty(ques_focal),
   disp('One focal used');
   one_f=1;
else
   disp('Two focals used');
   one_f=0;
end;

 
two_focal = ~one_f;

% Least squares on the radial distortion factor:

options = [1 1e-4 1e-4 1e-6  0 0 0 0 0 0 0 0 0 4000 0 1e-8 0.1 0];

if 0,
   disp('Minimizing over the radial distortion factor...');
  
   k = leastsq('error_explicit',0,options,[],grid_pts_centered,n_sq_x,n_sq_y,Np,W,L,Xgrid_2,two_focal);
else
   if quest_distort,
      k = k_g;
   else
      k=0;
   end;
end;

%keyboard;

[f,R,T,Horizon,distance,Vanish_vert,Vanish_hori,x_all_c]=distor_2_calib(k,grid_pts_centered,n_sq_x,n_sq_y,Np,W,L,Xgrid_2,100,2,two_focal);

Xc_2 = [x_all_c;ones(1,Np)].*(ones(3,1)*(distance./sum([x_all_c;ones(1,Np)].*(Horizon * ones(1,Np)))));

Xo_2 = R'*(Xc_2 - T*ones(1,Np));

XXo_2 = Xo_2 - mean(Xo_2')'*ones(1,Np);

ex = (Xgrid_2(1:2,:) - XXo_2(1:2,:));

figure(4);
plot(XXo_2(1,:),XXo_2(2,:),'bo'); hold on;
plot(Xgrid_2(1,:),Xgrid_2(2,:),'r+');
title('The reconstructed planar structure: o=true points, +=reconstructed');
xlabel('X (in cm)');
ylabel('Y (in cm)');
axis('ij');
axis('equal');
hold off;

%figure(5);
%plot(ex(1,:),ex(2,:),'r+');
%title('The error in the structure');
%xlabel('X (in cm)');
%xlabel('Y (in cm)');
%axis('ij');
%axis('equal');
%hold off;

rodrigues='rodrigues';
omega = feval(rodrigues,R);

T = Xc_2(:,ind_orig);

fprintf(1,'\n\nExplicit calibration parameters (initial guess):\n\n')
fprintf(1,'Rotation:     omc = [%3.5f   %3.5f   %3.5f]\n',omega)
fprintf(1,'Translation:   Tc = [%3.5f   %3.5f   %3.5f]\n',T)
fprintf(1,'Plane Normal:  nc = [%3.5f   %3.5f   %3.5f]\n',Horizon)
fprintf(1,'Distance:      dc = [%3.5f]\n',distance)
if length(f) == 2,
fprintf(1,'Focal Length:  fc = [%3.5f   %3.5f]\n',f)
else
fprintf(1,'Focal Length:  fc = [%3.5f]\n',f)
end;
fprintf(1,'Center:        cc = [%3.5f   %3.5f]\n',c)
fprintf(1,'Distortion:    kc = [%3.5f]\n\n\n',k)
fprintf(1,'Av. Dist. Err: ERREUR = %3.5f cm\n\n\n',mean(sqrt(sum(ex.^2))))


%%%%%%%% Second calibration using all points and the radial distortion:


x = grid_pts;
X = Xgrid;

Is_CD='is_cd';

if ~feval(Is_CD),
   
   disp('Save camera data (for further calibration purposes)');
   save camera_data x X 
   
else
   
   disp('Cannot save camera data on CD');

end;

if one_f,
      f = f(1);
end;


init_param2 = [omega' T' k f']; 	%% Now insert k

fprintf(1,'\nOptimization using all the points (no center)...\n')

 param = leastsq('image_error',init_param2,options,[],X,x,c);
%param = leastsq('image_error',init_param2,options,[],X,x,c2);


omega = param(1:3)';
T = param(4:6)';
k = param(7);
f = param(8:length(param))';
%c = c2;

rodrigues='rodrigues';
R = feval(rodrigues,omega); 

if length(f) < 2,
   f = [f;f];
end;


Horizon = R(:,3);

distance = dot(T,Horizon);

fprintf(1,'\n\nFinal Camera calibration parameters:\n\n')
fprintf(1,'Rotation:     omc = [%3.5f   %3.5f   %3.5f]\n',omega)
fprintf(1,'Translation:   Tc = [%3.5f   %3.5f   %3.5f]\n',T)
fprintf(1,'Plane Normal:  nc = [%3.5f   %3.5f   %3.5f]\n',Horizon)
fprintf(1,'Distance:      dc = [%3.5f]\n',distance)
fprintf(1,'Focal Length:  fc = [%3.5f   %3.5f]\n',f)
fprintf(1,'Center:        cc = [%3.5f   %3.5f]\n',c)
fprintf(1,'Distortion:    kc = [%3.5f]\n\n\n',k)


figure(2);
image(I); colormap(map);
hold on;
project2='project2';
y = feval(project2,X,R,T,f,c,k);
hy = plot(y(1,:)+1,y(2,:)+1,'oy');
%hy = plot(y(1,:)+1,y(2,:)+1,'ok');
set(hy,'linewidth',2,'MarkerSize',10);
hx = plot(x(1,:)+1,x(2,:)+1,'+r');
%hx = plot(x(1,:)+1,x(2,:)+1,'+k');
set(hx,'linewidth',2,'MarkerSize',10);
%hc = plot(c(1)+1,c(2)+1,'*m');
%title('Camera calibration image: + : Data points,  o : Projections');
drawnow;
clean_ima;
%clean_fig;
hold off;

clear figure(2)
%keyboard

%ex = x - y;

grid_pts_centered = [grid_pts(1,:) - c(1);grid_pts(2,:) - c(2)];

x_all_c = [grid_pts_centered(1,:)/f(1);grid_pts_centered(2,:)/f(2)];

comp_distortion='comp_distortion';

%%%%% X_all_c are the coordinate in pixel of the checkboard cross 
%%%%% points after distortion compensation 
x_all_c = feval(comp_distortion,x_all_c,k); % we can this time!!!

%%%%%% Xc_2 are the checkboard cross points in the camera ref. system
Xc_2 =[x_all_c;ones(1,Np)].*(ones(3,1)*(dot(R(:,3),T)./sum([x_all_c;ones(1,Np)].*(R(:,3)* ones(1,Np)))));

%%%%%%%%%%%%%% debug...
Np;
sil1=[x_all_c;ones(1,Np)];
sil3=sum([x_all_c;ones(1,Np)].*(R(:,3)* ones(1,Np)));
sil4=(dot(R(:,3),T)./sum([x_all_c;ones(1,Np)].*(R(:,3)* ones(1,Np))));
sil5=(ones(3,1)*(dot(R(:,3),T)./sum([x_all_c;ones(1,Np)].*(R(:,3)* ones(1,Np)))));
%%%%%%%%%%%%%%55

%%%%%%%% compute distance camera-ground plane
hh=dot(R(:,3),T);
fprintf('distance  of the camera to the ground plane: %f\n',hh);

%%%%%%%%  Xo_2 are the checkboard cross points in the world ref. system
Xo_2 = R'*(Xc_2 - T*ones(1,Np));

%[Xc_2_test, Xo_2_test] = pixel_2_3Dpoint(grid_pts(:,1),c,f,k,R,T);

%Xc_2_test(:,1)
%Xo_2_test(:,1)
%Xc_2(:,1)
%Xo_2(:,1)


%%%%%%%%%%%%%  estimate error
XXo_2 = Xo_2 - mean(Xo_2')'*ones(1,Np);

Xgrid_2 = Xgrid - mean(Xgrid')'*ones(1,Np);

ex = (Xgrid_2(1:2,:) - XXo_2(1:2,:));

fprintf(1,'Av. Dist. Err: ERREUR = %3.5f cm\n\n\n',mean(sqrt(sum(ex.^2))))

figure(4);
plot(XXo_2(1,:),XXo_2(2,:),'bo'); hold on;
plot(Xgrid_2(1,:),Xgrid_2(2,:),'r+');
title('The reconstructed planar structure: +=true points, o=reconstructed');
xlabel('X (in cm)');
ylabel('Y (in cm)');
axis('ij');
axis('equal');
hold off;

%figure(5);
%plot(ex(1,:),ex(2,:),'r+');
%title('The error in the structure');
%xlabel('X (in cm)');
%ylabel('Y (in cm)');
%axis('ij');
%axis('equal');
%hold off;

quest_satis = input('Satisfied? ([]=yes, other=no) ');

if isempty(quest_satis),
   satis_global = 1;
else
   satis_global = 0;
end;   

total = [omega(1) omega(2) omega(3); T(1) T(2) T(3); Horizon(1) Horizon(2) Horizon(3); distance 0 0; f(1) f(2) 0; c(1) c(2) 0; k 0 0];

end; % loop on the focal question