%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fit calibration image data from multiple cameras
% to a pinhole camera model, with allowance made for uncertainty in the
% position and rotation of the calibration plate
% 

calib_file_pat		= 'F:/bigtank-jul-2013/CAL-01-08-13/CAMERA/C-CAM%d-%02d.mat';
n_cameras			= 3;
n_views				= 21;
pl_to_gl_vec		= [-0.0014    1.0000   -0.0014    1.5540];
max_pos_error		= 0.5; % maximum position error, mm
max_ang_error		= 0.05;% maximum angle error, rad
n_iterations		= 10;

%% build setup structure

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

pl_to_gl						= vrrotvec2mat(pl_to_gl_vec);
re_plate_origin			= zeros(2, n_views);

camera						= struct();
camera.view					= struct('uv_markers', [], ...
											'ij_markers', []);
camera.view(1:n_views)	= camera.view;
setup							= struct('pl_to_gl', pl_to_gl, ...
											're_plate_origin', re_plate_origin, ...
											'omega', eye(3), ...
											're_plate_dx', zeros(2, n_views), ...
											'n_cameras', n_cameras, ...
											'n_views', n_views, ...
											'camera', camera);

for cam_idx = 1 : n_cameras
	camera		= struct();
	camera.view = struct('uv_markers', [], ...
								'ij_markers', []);
	camera.view(1:n_views) = camera.view;
	
	for view_idx = 1 : n_views
		calib_file		= sprintf(calib_file_pat, cam_idx, view_idx);
		f					= importdata(calib_file);
		
		camera.view(view_idx) = ...
			struct('uv_markers', f.xy_markers, ...
					 'ij_markers', f.ij_markers);
				 
		re_plate_origin(:, view_idx) = f.plate_pos;
	end
	
	setup.camera(cam_idx) = camera;
end
setup.re_plate_origin	= re_plate_origin;
original_setup = setup;
fprintf('done\n');

%% fit pinhole model for initial setup
[sCamCalib,res_before]	= pinhole_camera_fit(setup);

%% iterate

for it = 1 : n_iterations
	%% calculate initial camera calibration
	% assume no perturbation to plate position 
	fprintf('Iteration %d of %d\n', it, n_iterations);
	fprintf('   fitting pinhole camera model ... \n');
	[sCamCalib, res]	= pinhole_camera_fit(setup);
	fprintf('   RMS of i residual:   %0.3f\n', rms(res(1,:)));
	fprintf('   RMS of j residual:   %0.3f\n', rms(res(2,:)));
	for cam_idx = 1 : n_cameras
		setup.camera(cam_idx).calib = sCamCalib(cam_idx);
	end

	%% calculate model of error

	fprintf('   fitting model of error ... ');
	new_setup				= traverse_error_fit(setup);
	fprintf('done\n');
	
	%% update setup
	setup.re_plate_origin= new_setup.re_plate_origin + new_setup.re_plate_dx;
	setup.pl_to_gl			= new_setup.omega * new_setup.pl_to_gl;
	setup.omega				= eye(3);
	setup.re_plate_dx		= zeros(2, n_views);
end

%% fit pinhole model for final setup
[sNewCalib, res_after]	= pinhole_camera_fit(setup);
re_plate_dx					= setup.re_plate_origin - original_setup.re_plate_origin;
omega							= setup.pl_to_gl * original_setup.pl_to_gl^-1;
omega_vec					= vrrotmat2vec(omega);
theta							= omega_vec(4);
omega_axis					= omega_vec(1:3);

%% report
fprintf('------------------------------------------------------------------\n');
fprintf('Traverse position error modelling complete\n');
fprintf('\n');
fprintf('Before fit:\n');
fprintf('   RMS of i residual:   %0.3f\n', rms(res_before(1,:)));
fprintf('   RMS of j residual:   %0.3f\n', rms(res_before(2,:)));
fprintf('After fit:\n');
fprintf('   RMS of i residual:   %0.3f\n', rms(res_after(1,:)));
fprintf('   RMS of j residual:   %0.3f\n', rms(res_after(2,:)));

fprintf('Rotation axis:       %0.3f %0.3f %0.3f\n', omega_axis(1:3));
fprintf('Rotation angle:      %0.3f mrad\n', theta*1000);
figure(1);
quiver(re_plate_origin(1,:), re_plate_origin(2,:), ...
		 re_plate_dx(1,:),     re_plate_dx(2,:), 'k-');
axis equal tight;
xlabel('Traverse x position / mm');
ylabel('Traverse z position / mm');
title('Estimated error in traverse position');
