%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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/']);
%% input parameters
camera_model_file			= 'L:\array\tcf\6C_A\calib\camera-model.mat';
font_size					= 20;

%% load model
setup						= importdata(camera_model_file);

% camera calibration
n_cameras					= setup.n_cameras;
n_views						= setup.n_views;

% plate pose
gl_plate_pos				= setup.gl_plate_pos;
pl_to_gl_mat				= repmat(setup.pl_to_gl, [1 1 n_views]);
camera_model				= setup.camera;
gl_to_re					= eye(3);
gl_origin					= setup.gl_origin;
r_sphere					= min(setup.r_sphere);

%% get bounding frustums
M_ary						= zeros(4,4,n_cameras);		% equation of plane
U_ary						= zeros(3,4,n_cameras);		% unit vectors of viewing pyramid edges
xc_ary						= zeros(3,n_cameras);		% camera origin
xf_ary						= zeros(3,4,n_cameras);		% points on corners of viewing pyramid
for c = 1 : n_cameras
	[M,U,xc]					= frustum(setup.camera(c));
	M_ary(:,:,c)			= M;
	U_ary(:,:,c)			= U;
	xc_ary(:,c)				= xc;
	% get four corners of view frustum
	for p = 1 : 4
		len					= dot(gl_origin - xc, U(:,p));
		xf_ary(:,p,c)		= xc + (len + 2*r_sphere)*U(:,p);
	end
end

%% determine bounding boxes
[gl_bb_lim, gl_bb_A, gl_bb_b]= bounding_box(setup.camera);
[xg,yg,zg]				= ndgrid(linspace(gl_bb_lim(1,1), gl_bb_lim(2,1), 100), ...
										linspace(gl_bb_lim(1,2), gl_bb_lim(2,2), 100), ...
										linspace(gl_bb_lim(1,3), gl_bb_lim(2,3), 100));
gl_pts					= [xg(:), yg(:), zg(:)]';
bb_dist					= bsxfun(@minus, gl_bb_A*gl_pts, gl_bb_b);
b_inside					= all(bb_dist <= 0, 1);
gl_inside				= gl_pts(:,b_inside);

% determine minimum bounding box
[mv_bb_lim, mv_to_gl, mv_bb_A, mv_bb_b] = min_bounding_box(setup.camera);
mv_bb_size				= mv_bb_lim(2,:) - mv_bb_lim(1,:);

% save in setup
setup.gl_bb_lim			= gl_bb_lim;
setup.gl_bb_A			= gl_bb_A;
setup.gl_bb_b			= gl_bb_b;
setup.mv_bb_lim			= mv_bb_lim;
setup.mv_to_gl			= mv_to_gl;
setup.mv_bb_A			= mv_bb_A;
setup.mv_bb_b			= mv_bb_b;

%% convex bounding hull
[tri_hull, V_hull]	= convhull(gl_inside(1,:)', gl_inside(2,:)', gl_inside(3,:)');
fprintf('Convex hull has volume %0.1f mm^2\n', V_hull);

%% calculate coordinates of inscribed sphere
theta							= linspace(0, pi, 21);
phi							= linspace(0, 2*pi, 41);
[tmat,pmat]					= ndgrid(theta,phi);
en_sphere					= [sin(tmat(:)').*cos(pmat(:)');
									sin(tmat(:)').*sin(pmat(:)');
									cos(tmat(:)')];
gl_sphere					= bsxfun(@plus, r_sphere * en_sphere, gl_origin);

%% project inscribed sphere onto different views
n_sphere						= size(gl_sphere, 2);
ij_sphere_ary				= zeros(2, n_sphere, n_cameras);
ij_orgin_ary				= zeros(2, n_cameras);
for c = 1 : n_cameras
	ij_sphere_ary(:,:,c) = pinhole_model(gl_sphere, setup.camera(c).calib, true);
	ij_origin_ary(:,c)	= pinhole_model(gl_origin, setup.camera(c).calib, true);
end

%% plot 3D view of frustum and inscribed sphere
figure(1);
clf;
hold on;

% inscribed sphere
plot3(gl_sphere(1,:), gl_sphere(2,:), gl_sphere(3,:), 'k.');
% convex bounding hull
S = trisurf(tri_hull, gl_inside(1,:), gl_inside(2,:), gl_inside(3,:));
set(S, 'FaceColor', 'k', 'facealpha', 0.3, 'edgecolor', 'none');

% viewing frustum of each camera
colours						= {'red', 'green', 'blue'};
for c = 1 : n_cameras
	plot3(xc_ary(1,c), xc_ary(2,c), xc_ary(3,c), 'o', 'Color', colours{c});
	for p = 1 : 4
		tri					= [xc_ary(:,c), xf_ary(:,p,c), xf_ary(:, mod(p,4)+1, c)];
		P						= patch(tri(1,:), tri(2,:), tri(3,:), colours{c});
		set(P, 'facealpha', 0.2);
	end
end

set(gca, 'CameraTarget', gl_origin);
% plot settings
axis equal tight xy
view(3);
%xlim(gl_origin(1) + 3*r_sphere*[-1 1]);
%ylim(gl_origin(2) + 3*r_sphere*[-1 1]);
%zlim(gl_origin(3) + 3*r_sphere*[-1 1]);
xlabel('x [mm]', 'interpreter', 'latex', 'fontsize', font_size);
ylabel('y [mm]', 'interpreter', 'latex', 'fontsize', font_size);
zlabel('z [mm]', 'interpreter', 'latex', 'fontsize', font_size);

%% plot the projection in each camera
figure(2);
clf;

for c = 1 : n_cameras
	%% bits
	i_axis				= setup.camera(c).i_axis;
	j_axis				= setup.camera(c).j_axis;
	ij_sphere			= ij_sphere_ary(:,:,c);
	ij_origin			= ij_origin_ary(:,c);
	id						= setup.camera(c).id;
	titstr				= sprintf('Camera %d, id = %d', c, id);
	
	%% plot
	subplot(2,2,c);
	cla;
	hold on;
	plot(ij_sphere(1,:), ij_sphere(2,:), 'k.');
	plot(ij_origin(1), ij_origin(2), 'ro');
	axis equal tight xy;
	xlim(sort(i_axis([1 end])));
	ylim(sort(j_axis([1 end])));
	title(titstr, 'interpreter', 'latex', 'fontsize', font_size);
	xlabel('i / px', 'interpreter', 'latex', 'fontsize', font_size);
	xlabel('j / px', 'interpreter', 'latex', 'fontsize', font_size);
end

%% calculate marker positions for each view
re_marker_ary			= cell(1,2);

for c = 1 : 2
	for v = 1 : n_views
		uv_markers	= camera_model(c).view(v).uv_markers;
		n_markers	= size(uv_markers, 2);
		%% append to vectors
		gl_pos		= gl_plate_pos(:,v);
		pl_to_gl		= pl_to_gl_mat(:,:,v);

		u_marker		= uv_markers(1,:);
		v_marker		= uv_markers(2,:);
		gl_markers	= bsxfun(@plus, gl_pos, pl_to_gl * [u_marker; v_marker; zeros(1,n_markers)]);
		re_markers	= gl_to_re * bsxfun(@plus, -gl_origin, gl_markers); 

		re_marker_ary{c} = [re_marker_ary{c}, re_markers];
	end
end

% find markers in camera 1 only, camera 2 only and both
re_marker_c1		= setdiff(re_marker_ary{1}', re_marker_ary{2}', 'rows')';
re_marker_c2		= setdiff(re_marker_ary{2}', re_marker_ary{1}', 'rows')';
re_marker_c12		= intersect(re_marker_ary{1}', re_marker_ary{2}', 'rows')';

%% plot
figure(10);
clf;
hold on;

% plot markers
plot3(re_marker_c1(1,:), re_marker_c1(2,:), re_marker_c1(3,:), '.', 'color', [1 0 0]);
plot3(re_marker_c2(1,:), re_marker_c2(2,:), re_marker_c2(3,:), '.', 'color', [0 0 1]);
plot3(re_marker_c12(1,:), re_marker_c12(2,:), re_marker_c12(3,:), '.', 'color', [0.8 0 0.8]);

axis equal tight xy;
view(3);
camup([0 1 0]);

%xlabel('$x$ / mm', 'interpreter', 'latex', 'fontsize', 20);
%ylabel('$y$ / mm', 'interpreter', 'latex', 'fontsize', 20);
%zlabel('$z$ / mm', 'interpreter', 'latex', 'fontsize', 20);