%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% camera_calibration.m
%
% create a pinhole camera calibration for one or more cameras
% from a set of views of the same calibration plate in different positions
% a compensation is made for error in calibration plate position and
% orientation

addpath([pwd '/pinhole/']);
addpath([pwd '/../../misc/']);
addpath([pwd '/../../misc/mtimesx/']);
addpath([pwd '/../../misc/mmx'])
addpath([pwd '/../matching/']);

%% input parameters

calib_dir					= 'E:\mixer\lpt\calibration\21-07-09';
cam_calib_file_pat          = '%s/CAM%d-%02d.mat';
cam_model_file				= [calib_dir '/camera-model.nrd.mat'];

cam_index_range             = [1 2 3];
cam_view_range				= [1:6];%1 : 6;% : 11;
n_views						= length(cam_view_range);
n_cameras					= length(cam_index_range);
n_iterations				= 2;

b_debug_plot				= true;
b_radial_distortion			= false;

%% estimate of plate orientation relative to global coordinate system
plate_eu						= [1 0 0];
plate_ev						= [0 1 0];
plate_ew						= [0 0 1];
pl_to_gl						= [plate_eu(:) plate_ev(:) plate_ew(:)];

%% build setup structure

fprintf('Loading data from file ... ');

gl_plate_pos				= zeros(3, n_views);
pl_to_gl_mat				= repmat(pl_to_gl, [1 1 n_views]);
camera						= struct('i_axis', [], 'j_axis', [], ...
											'id', [], 'view', []);
camera.view					= struct('uv_markers', [], ...
											'ij_markers', []);
camera.view(1:n_views)		= camera.view;
emptyCamera					= camera;

setup							= struct('pl_to_gl', pl_to_gl, ...
										'pl_to_gl_mat', pl_to_gl_mat, ...
										'gl_plate_pos', gl_plate_pos, ...
										'omega', eye(3), ...
										'gl_plate_dx', zeros(3, n_views), ...
										'gl_plate_omega', zeros(3, n_views), ...
										'n_cameras', n_cameras, ...
										'n_views', n_views, ...
										'camera', camera, ...
										'b_radial_distortion', b_radial_distortion);

for c = 1 : n_cameras
	camera					= emptyCamera;
	
	for view_idx = 1 : n_views
		calib_file			= sprintf(cam_calib_file_pat, calib_dir, cam_index_range(c), cam_view_range(view_idx));
		f						= importdata(calib_file);
		
		camera.view(view_idx) = ...
			struct('uv_markers', f.xy_markers, ...
					 'ij_markers', f.ij_markers);
	end
	
	camera.id				= f.id;
	camera.i_axis			= f.i_axis;
	camera.j_axis			= f.j_axis;
	setup.camera(c)= camera;
end

for view_idx = 1 : n_views
	calib_file				= sprintf(cam_calib_file_pat, calib_dir, cam_index_range(c), cam_view_range(view_idx));
	f							= importdata(calib_file);
	gl_plate_pos(:, view_idx) = f.plate_pos;
end
% enforce barycentre of plate positions to be zero
gl_plate_pos		= bsxfun(@minus, gl_plate_pos, mean(gl_plate_pos,2));
setup.gl_plate_pos	= gl_plate_pos;
fprintf('done\n');

%% check markers 
figure(100);
for n = 1 : n_cameras
    %% load coordinates of markers from setup
    camera			= setup.camera(n);	
    [gl_markers, ij_markers] = plate_to_world(camera, setup);
    subplot(1,n_cameras,n);
    z_unique        = unique(gl_markers(3,:));
    n_z             = length(z_unique);
    cmap            = jet(n_z);
    for k = 1 : n_z
        rng         = z_unique(k) == gl_markers(3,:);
        plot(ij_markers(1,rng), ij_markers(2,rng), '.', 'color', cmap(k,:));
        hold on;
    end
    axis equal tight xy;
    xlim(sort(camera.i_axis([1 end])));
    ylim(sort(camera.j_axis([1 end])));
end

%% estimate the intrinsic/extrinsic parameters using MATLAB routines
setup.camera		= num2cell(setup.camera);
for c = 1 : n_cameras
	fprintf('   camera %d:\n', c);
	camera			= setup.camera{c};
	im_size			= [length(camera.i_axis) length(camera.j_axis)];
	
	%% extract unique points
	uv_mark			= camera.view.uv_markers;
	u_vec			= unique(uv_mark(1,:));
	v_vec			= unique(uv_mark(2,:));
	n_u				= length(u_vec);
	n_v				= length(v_vec);
	n_markers		= n_u * n_v;
	[um,vm]			= ndgrid(u_vec, v_vec);
	uv_grid			= [um(:) vm(:)]';
	
	fprintf('    %d x %d board\n', n_u, n_v);
	
	%% load into array
	ij_mark				= nan(2, n_markers, n_views);
	for v = 1 : n_views
		uv_found		= camera.view(v).uv_markers;
		ij_found		= camera.view(v).ij_markers;
		k				= knnsearch(uv_grid', uv_found');
		ij_mark(:,k,v)	= ij_found;
	end
	
	%% estimate intrinsic parameters
	[cparams, img_used, err] = estimateCameraParameters(permute(ij_mark,[2 1 3]), uv_grid', ...
									'ImageSize', im_size, ...
									'NumRadialDistortionCoefficients', 2);
	
	%% estimate orientation 
	[gl_mark, ij_mark]	= plate_to_world(camera, setup);
	[orient, loc]		= estimateWorldCameraPose(ij_mark', gl_mark', cparams);
	
	%% store in structure
	camera.Orientation	= orient;
	camera.Location		= loc;
	%camera.Intrinsics	= cparams.Intrinsics;
	camera.cparams		= cparams;
	camera.gl_mark		= gl_mark;
	camera.ij_mark		= ij_mark;
	
	setup.camera{c}		= camera;
end

%{
%% MATLAB estimation of camera parameters
fprintf('fitting camera model using MATLAB routines:\n');
for c = 1 : n_cameras
	fprintf('   camera %d:\n', c);
	camera			= setup.camera(c);
	im_size			= [length(camera.i_axis) length(camera.j_axis)];
	
	%% extract unique points
	uv_mark			= camera.view.uv_markers;
	u_vec			= unique(uv_mark(1,:));
	v_vec			= unique(uv_mark(2,:));
	n_u				= length(u_vec);
	n_v				= length(v_vec);
	n_markers		= n_u * n_v;
	[um,vm]			= ndgrid(u_vec, v_vec);
	uv_grid			= [um(:) vm(:)]';
	
	fprintf('    %d x %d board\n', n_u, n_v);
	
	%% load into array
	ij_mark				= nan(2, n_markers, n_views);
	for v = 1 : n_views
		uv_found		= setup.camera(c).view(v).uv_markers;
		ij_found		= setup.camera(c).view(v).ij_markers;
		k				= knnsearch(uv_grid', uv_found');
		ij_mark(:,k,v)	= ij_found;
	end
	
	
	G					= cparams.IntrinsicMatrix';
	cx					= G(1,3);
	skew				= G(1,2);
	fx					= G(1,1);
	fy					= G(2,2);
	cy					= G(2,3);
	ic					= [cx cy];
	kr					= [0 0];
	
	%{
	intrinsics			= cam_params.Intrinsics;
	fx					= intrinsics.FocalLength(1);
	fy					= intrinsics.FocalLength(2);
	skew				= intrinsics.Skew;
	cx					= intrinsics.PrincipalPoint(1);
	cy					= intrinsics.PrincipalPoint(2);
	ic					= [cx cy];
	f					= sqrt(fx*fy);
	%kr					= intrinsics.RadialDistortion.*[f^-2 f^-4];
	kr					= [0 0];
	G					= [fx skew cx; 0 fy cy; 0 0 1];
	%G					= cam_params.IntrinsicMatrix';
	%}
	
	%% perform a bundle adjustment
	[gl_mark, ij_mark]	= plate_to_world(camera, setup);
	% identify which view marker corresponds to
	[z_planes,~,id_view]= unique(gl_mark(3,:));
	% identify which calibration target point each marker corresponds to
	[uv_unique,~,id_mark]= unique(gl_mark(1:2,:)', 'rows');
	
	
	%% estimate extrinsic parameters
	[orient, loc]		= estimateWorldCameraPose(ij_mark', gl_mark', cparams);
	[Rt, t]				= cameraPoseToExtrinsics(orient, loc);
	xc					= loc(:);
	R					= orient;
	%R					= Rt';
	% check: xc = -Rt*t(:) 
	% check: R*Rt = eye(3)
	% disp(norm(-Rt*t(:)-xc))
	% disp(norm(R*Rt - eye(3)))
	
	%% convert to camera calibration structure
	calib				= struct();
	calib.xc			= xc;
	calib.R				= R;
	calib.G				= G;
	calib.P				= G*R*[eye(3) -xc];
	calib.ic			= ic;
	calib.kr			= kr;
	setup.camera(c).calib=calib;
	disp(G);
	
	%% measure reprojection error
	ij_dewarped			= undistortPoints(ij_mark', cparams)';
	ij_proj				= pinhole_model(gl_mark, calib, false);
	%ij_res				= ij_proj - ij_dewarped;
	ij_res				= ij_proj - ij_mark;
	ij_res_rms			= rms(ij_res, 2);
	
	fprintf('model residual: %0.3f %0.3f px\n', ij_res_rms);
end
old_setup = setup;

%% plot estimate of extrinsic parameters
figure(1);
clf;
plot3(gl_mark(1,:), gl_mark(2,:), gl_mark(3,:), 'r.')
hold on
axis equal tight
cmap=eye(3);
for c = 1 : 3
	calib = setup.camera(c).calib;
	plot3(calib.xc(1), calib.xc(2), calib.xc(3), 'o', 'color', cmap(c,:))
end
zlabel('z');
ylabel('y');
xlabel('x');
%}