%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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;
b_estimate_skew				= false;
b_tilt_shift				= true;

%% 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, ...
										'b_estimate_skew', b_estimate_skew, ...
										'b_tilt_shift', b_tilt_shift);

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

%% estimate intrinsics
camera		= pinhole_fit_zhang(setup.camera(2), setup);
disp(camera.calib.G);
