%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 '/../core/pinhole/']);
addpath([pwd '/../../misc/']);
addpath([pwd '/../../misc/mtimesx/']);
addpath([pwd '/../../misc/mmx'])
addpath([pwd '/../matching/']);

%% input parameters

calib_dir					= 'F:\mixer\lpt\calibration\21-06-30';
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 : 11;
n_views						= length(cam_view_range);
n_cameras					= length(cam_index_range);
n_iterations				= 0;

b_debug_plot				= true;

%% optimisation constraints
constraints						= struct();
constraints.b_square			= false;	% square pixels?
constraints.b_estimate_skew		= true;		% allow skew?
constraints.b_radial_distortion = false;	% use second order radial distortion? (ignore if algebraic algorithm used)
constraints.b_tilt_shift		= false;	% no tilt shift adapter, radial distortion is coaxial with image plane (ignore if no radial distortion)
constraints.b_minimise_algebraic= true;		% minimise algebraic error rather than geometric error

%% 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);

for cam_idx = 1 : n_cameras
	camera					= emptyCamera;
	
	for view_idx = 1 : n_views
		calib_file			= sprintf(cam_calib_file_pat, calib_dir, cam_index_range(cam_idx), 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(cam_idx)= camera;
end

for view_idx = 1 : n_views
	calib_file				= sprintf(cam_calib_file_pat, calib_dir, cam_index_range(cam_idx), 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

%% pinhole fit before bundle calibration
fprintf('fitting camera model to setup before bundle calibration:\n');
[s_old_calib,res_before]= pinhole_camera_fit(setup, constraints);
fprintf('fit quality before bundle calibration:\n');
for cam_idx = 1 : n_cameras
	
	fprintf('   camera %d:\n', cam_idx);
	fprintf('      RMS of i residual:   %0.3f px\n', rms(res_before{cam_idx}(1,:)));
	fprintf('      RMS of j residual:   %0.3f px\n', rms(res_before{cam_idx}(2,:)));
	%fprintf('      RMS of x residual:   %0.3f mm\n', rms(
	setup.camera(cam_idx).calib = s_old_calib(cam_idx);
end
old_setup = setup;

%% iterate error fitting
% 1) use pinhole model to esimate error in plate position
% 2) update model of plate position
% 3) fit a pinhole camera model
% 4) go to 1 if iteration limit not exceeded
for it = 1 : n_iterations
	%% calculate model of error
	fprintf('   fitting model of error ... ');
	new_setup					= traverse_error_fit(setup);
	fprintf('done\n');
	
	%% apply correction
	new_setup.gl_plate_pos	= new_setup.gl_plate_pos + new_setup.gl_plate_dx;
	%new_setup.pl_to_gl		= new_setup.omega * new_setup.pl_to_gl;
	for v = 1 : n_views
		omega_vec			= new_setup.gl_plate_omega(:,v);
		Omega				= vrrotvec2mat([omega_vec(:)/(norm(omega_vec)+eps); norm(omega_vec)]);
		new_setup.pl_to_gl_mat(:,:,v) = Omega * new_setup.pl_to_gl_mat(:,:,v);
	end	
	new_setup.omega			= eye(3);
	new_setup.gl_plate_dx	= zeros(3, n_views);
	new_setup.gl_plate_omega= zeros(3, n_views);
	
	%% calculate new camera calibration
	% assume no perturbation to plate position 
	fprintf('Iteration %d of %d\n', it, n_iterations);
	fprintf('   fitting pinhole camera models ... \n');
	[s_cam_calib, res]		= pinhole_camera_fit(new_setup, constraints);
	
	for cam_idx = 1 : n_cameras
		fprintf('   camera %d:\n', cam_idx);
		fprintf('      RMS of i residual:   %0.3f\n', rms(res{cam_idx}(1,:)));
		fprintf('      RMS of j residual:   %0.3f\n', rms(res{cam_idx}(2,:)));
	end
	
	%% fix reference frame
	% stop setup shuffling about by enforcing barycentre of plate positions
	% to be origin
	origin_shift				= -mean(new_setup.gl_plate_pos, 2);
	new_setup.gl_plate_pos	= bsxfun(@plus, new_setup.gl_plate_pos, origin_shift);
	for cam_idx = 1 : n_cameras
		calib						= s_cam_calib(cam_idx);
		calib.xc					= calib.xc(:) + origin_shift;
		calib.P					= calib.G * calib.R * [eye(3), -calib.xc(:)];
		new_setup.camera(cam_idx).calib = calib;
	end
	
	%% update
	setup					= new_setup;
end
new_setup				= setup;

%% pinhole fit for final setup
fprintf('fitting camera model to new setup:\n');
[s_new_calib, res_after]= pinhole_camera_fit(new_setup, constraints);
fprintf('fit quality after bundle calibration:\n');
for cam_idx = 1 : n_cameras
	fprintf('   camera %d:\n', cam_idx);
	fprintf('      RMS of i residual:   %0.3f\n', rms(res_after{cam_idx}(1,:)));
	fprintf('      RMS of j residual:   %0.3f\n', rms(res_after{cam_idx}(2,:)));
	setup.camera(cam_idx).calib = s_new_calib(cam_idx);
end

%% determine size + center of volume
% bounding volume
[gl_origin, r_sphere]	= volume_bounds(new_setup);
r_sphere_lim				= min(r_sphere);
% compute bounding box
[gl_bb_lim, gl_bb_A, gl_bb_b]= bounding_box(new_setup.camera);

% add to setup
new_setup.gl_origin		= gl_origin;
new_setup.r_sphere		= r_sphere;
new_setup.r_sphere_lim	= r_sphere_lim;
new_setup.gl_bb_lim		= gl_bb_lim;
new_setup.gl_bb_A		= gl_bb_A;
new_setup.gl_bb_b		= gl_bb_b;

%% difference compared to old setup
gl_plate_dx					= new_setup.gl_plate_pos - old_setup.gl_plate_pos;
gl_plate_omega				= zeros(4, n_views);
for v = 1 : n_views
	Omega						= new_setup.pl_to_gl_mat(:,:,v) / old_setup.pl_to_gl_mat(:,:,v);
	gl_plate_omega(:,v)	= vrrotmat2vec(Omega);
end
gl_plate_dtheta			= gl_plate_omega(4,:);
gl_plate_rotaxis			= gl_plate_omega(1:3,:);

%% report
fprintf('------------------------------------------------------------------\n');
fprintf('Traverse position error modelling complete\n');
fprintf('\n');
for c = 1 : n_cameras
	fprintf('Camera %d\n', c);
	fprintf('Before fit:   %0.3f %0.3f px rms %0.3f %0.3f px max\n', ...
		rms(res_before{c},2), max(abs(res_before{c}),[],2) );
	fprintf('After  fit:   %0.3f %0.3f px rms %0.3f %0.3f px max\n', ...
		rms(res_after{c},2), max(abs(res_after{c}),[],2) );
	fprintf('Largest inscribed sphere:\n');
	fprintf('   r = %0.2f mm, d = %0.2fmm\n', r_sphere(c), r_sphere(c)*2);
	fprintf('\n');
end
fprintf('Largest rotation angle: %0.3f mrad\n', max(abs(gl_plate_dtheta))*1000);
fprintf('Largest plate dx:			%0.3f\n', max(abs(gl_plate_dx(1,:))));
fprintf('Largest plate dy:			%0.3f\n', max(abs(gl_plate_dx(2,:))));
fprintf('Largest plate dz:			%0.3f\n', max(abs(gl_plate_dx(3,:))));


% report
fprintf('\n\n\n');
fprintf('Origin is at:\n');
fprintf('    %+0.5f\n', gl_origin);
fprintf('Inscribed sphere has dimension:\n');
fprintf('    r = %0.2f mm, d = %0.2f mm\n', r_sphere_lim, 2*r_sphere_lim);

%% save camera model to file
fprintf('Saving ... ');
save(cam_model_file, '-struct', 'new_setup');
fprintf('done\n');

%% plot error in traverse position
figure(10);
quiver(gl_plate_pos(1,:),		gl_plate_pos(3,:), ...
		 gl_plate_dx(1,:),		gl_plate_dx(3,:), 'k-');
axis equal tight;
xlabel('Traverse x position / mm');
ylabel('Traverse z position / mm');
title('Estimated error in traverse position');

%% plot to compare new and old setups
if b_debug_plot
	%% colours for different views
	view_colour			= jet(n_views);
	camera_colour		= hsv(n_cameras);
	
	%% 3d reconstruction
	figure(10);
	clf;
	hold on;
	for cam_idx = 1 : n_cameras
		gl_old	= plate_to_world(old_setup.camera(cam_idx), old_setup);
		gl_new	= plate_to_world(new_setup.camera(cam_idx), new_setup);
		plot3(gl_old(1,:), gl_old(2,:), gl_old(3,:), '.', 'color', camera_colour(cam_idx,:));
		plot3(gl_new(1,:), gl_new(2,:), gl_new(3,:), 'o', 'color', camera_colour(cam_idx,:));
	end
	axis equal tight xy;
	axis(gl_bb_lim(:));
	view(2);
	
	%% loop over setups
	setup_ary				= cell(1,2);
	setup_ary{1}			= old_setup;
	setup_ary{2}			= new_setup;
	
	for s = 1 : 2
		%% initialise
		setup					= setup_ary{s};
		figure(s);
		clf;
		subplot1(2,2,'Gap',[0.05 0.05]);
		%% plot each camera
		for cam_idx = 1 : n_cameras
			subplot1(cam_idx);
			cla;
			hold on;

			camera		= setup.camera(cam_idx);
			for v = n_views%[1 , n_views]
				uv_mark	= camera.view(v).uv_markers;
				n_mark	= size(uv_mark, 2);
				gl_plate	= setup.gl_plate_pos(:,v);
				gl_mark	= bsxfun(@plus, gl_plate, setup.pl_to_gl_mat(:,:,v)*[uv_mark; zeros(1,n_mark)]);

				ij_mark	= camera.view(v).ij_markers;
				ij_proj	= pinhole_model(gl_mark, camera.calib);

				plot(ij_mark(1,:), ij_mark(2,:), 'o', 'color', 'k'); %marker_colour(v,:));
				plot(ij_proj(1,:), ij_proj(2,:), 'x', 'color', 'r'); %marker_colour(v,:));
				%quiver(	ij_mark(1,:), ij_mark(2,:), ...
				%			ij_proj(1,:)-ij_mark(1,:), ij_mark(2,:)-ij_proj(2,:),'k-');
			end
			axis equal tight xy;
			xlim(sort(camera.i_axis([1 end])));
			ylim(sort(camera.j_axis([1 end])));
			make_plot_pretty('i / px', 'j / px', sprintf('Camera %d', cam_index_range(cam_idx)), 20);
			if cam_idx ~= 1
				ylabel('');
			end
			set(gca,'xtick',camera.i_axis(1):256:camera.i_axis(end));
			set(gca,'ytick',camera.j_axis(1):256:camera.j_axis(end));
		end
	end
end