added experimental stereo and calibration routines

calibration/automatic_filter.m

+cd%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% automatic_filter.m
+% 
+% utility to help find the best filter size for identifying markers
+% for a given image file, the Laplacian of Gaussian method for finding
+% markers will be applied and the centre three markers extracted
+% the filter scale can be adjusted to allow experimentation
+
+addpath([pwd '/../../mex/minmax/']);
+addpath([pwd '/../']);
+addpath([pwd '/pinhole/']);
+addpath([pwd '/../gmv/']);
+
+%% input parameters
+
+grid_dx				= 2.54;		% real units, mm
+grid_dy				= 2.54;
+filter_type			= 'disk';
+
+input_file			= '/nfs/scratch23/lawson/K62/170203/calib/POS01.cam1.tif';
+
+%% load image
+[~,~,ext]		= fileparts(input_file);
+if strcmpi(ext, '.gmv')
+	% loading of .gmv movie files
+	im					= mean(double(read_gmv_matlab(input_file)), 3);
+	im					= fliplr(im);	
+else
+	% loading of regular image files
+	im					= double(imread(input_file));
+	im					= fliplr(im');
+end
+Ni						= size(im, 1);
+Nj						= size(im, 2);
+i_axis				= 0 : Ni-1;
+j_axis				= 0 : Nj-1;
+
+%% loop
+while true
+	%% get filter scale
+	s_resp			= input('Enter filter scale in pixels or q to quit: ', 's');
+	if strcmp(s_resp, 'q')
+		break;
+	end
+	blob_scale		= str2double(s_resp);
+	if isnan(blob_scale)
+		continue;
+	end
+	
+	%% filter
+	[ij_small,im_filt]		= log_find_markers(i_axis, j_axis, im, blob_scale, 1000, filter_type);
+	[ij_big,im_filt_big]		= log_find_markers(i_axis, j_axis, im, 2*blob_scale, 3, filter_type);
+	
+	%% plot
+	figure(1);
+	clf;
+	imagesc(i_axis, j_axis, im_filt');
+	hold on;
+	plot(ij_small(1,:), ij_small(2,:), 'r.');
+	plot(ij_big(1,:), ij_big(2,:), 'ro');
+	axis equal tight xy;
+	colorbar;
+end
\ No newline at end of file

calibration/bounding_box.m

+function [bb_lim, A, b] = bounding_box(camera)
+	% bbmax = bounding_box(calib)
+	% determine the corners of the smallest axis aligned bounding box
+	% which fit encapsulate the extent of the measurement volume
+	%
+	% bbmax is a 2 x 3 matrix containing the x, y and z limits of the
+	% smallest bounding box which encapsulate the entire measurement volume
+	
+	%% get view frustums
+	% M = M_ary(:,p,c) are the coefficients for the equation of plane p of the
+	% viewing frustum for camera c. 
+	% i.e.
+	%		M(1)x + M(2)y + M(3)z + M(4) >= 0
+	% when a point lies within the measurement volume
+	n_cameras		= length(camera);
+	M_ary				= zeros(4,4,n_cameras);
+	for c = 1 : n_cameras
+		M_ary(:,:,c)= frustum(camera(c))';
+	end
+	
+	%% solve linear programming problem
+	% to obtain volume bounds, we find the x that 
+	% minimises c'x subject to Ax < b
+	A					= reshape(-M_ary(1:3,:,:), [3 4*n_cameras])';
+	b					= reshape( M_ary(4  ,:,:), [4*n_cameras 1]);
+	bb_lim			= zeros(2,3);
+	opts				= optimoptions('linprog', 'Display', 'off');
+	for i = 1 : 3
+		e_i			= [0 0 0]';
+		e_i(i)		= 1;
+		bb_lim(1,i)	= dot(e_i, linprog(+e_i, A, b, [], [], [], [], [], opts));
+		bb_lim(2,i)	= dot(e_i, linprog(-e_i, A, b, [], [], [], [], [], opts));
+	end
+	
+	%% test
+	%{
+	N					= 1E5;
+	bb_size			= bb_lim(2,:) - bb_lim(1,:);
+	gl_pos			= bsxfun(@plus, bb_lim(1,:)', diag(bb_size) * rand(3,N));
+	bb_dist			= bsxfun(@minus, A*gl_pos, b);
+	b_inside			= all(bb_dist <= 0, 1);
+	figure(1);
+	clf;
+	plot3(gl_pos(1, b_inside), gl_pos(2, b_inside), gl_pos(3, b_inside), 'g.');
+	hold on;
+	plot3(gl_pos(1,~b_inside), gl_pos(2,~b_inside), gl_pos(3,~b_inside), 'r.');
+	axis equal tight xy;
+	%}
+end
\ No newline at end of file

calibration/bounding_polygon.m

+function [A,b] = bounding_polygon(corners)
+	% [A,b] = bounding_polygon(corners)
+	% determine the coefficients of an inequality Ay <= b such that
+	%
+	% if Ay <= b
+	%	if polygon is convex:
+	%		y is inside polygon
+	%  else
+	%		y is inside the convex hull of the polygon
+	% else
+	%	y is outside polygon
+	%
+	
+	%% get convex hull
+	idx		= convhull(corners(1,:)', corners(2,:)');
+	corners	= corners(:,idx);
+	
+	%% get coefficients
+	n_vert	= size(corners,2)-1;
+	A			= zeros(n_vert,2);
+	b			= zeros(n_vert,1);
+	
+	for v = 1 : n_vert
+		% get normal of edge from y0 -> y1
+		y0		= corners(:, v);
+		y1		= corners(:, v+1);
+		y01	= y1 - y0;
+		n		= [y01(2); -y01(1)];
+		% we are inside if (y - y0).n <= 0
+		% i.e. n.y <= n.y0
+		A(v,:)= n';
+		b(v)	= dot(n, y0);
+	end
+end
\ No newline at end of file

calibration/bounding_polyhedron.m

+function [A_hull,b_hull] = bounding_polyhedron(tri, vert)
+	% [A,b] = bounding_polyhedron(tri,vert)
+	% 
+	% determine the system of linear constraints
+	%			Ax <= b
+	% that describe the inside of the convex polyhedron described by
+	% tri and vert. That is, if x lies within the polyhedron, Ax<=b
+	%
+	% vert is a 3 x n_vert array of vertex coordinates
+	% tri	is a n_face x 3 array of indices, describing the triangular
+	% faces of the polyhedron
+	
+	n_vert		= size(vert, 2);
+	n_face		= size(tri, 1);
+	
+	%% determine surface normals
+	% vertices of each triangular face
+	A				= vert(:, tri(:,1));
+	B				= vert(:, tri(:,2));
+	C				= vert(:, tri(:,3));
+	D				= mean(vert, 2);
+	DA				= bsxfun(@minus, A, D);
+	% surface normal
+	n				= cross(B-A, C-A, 1);
+	n				= bsxfun(@rdivide, n, sqrt(sum(n.^2,1)));
+	% sign of surface normal is outwards positive
+	% D is inside polyhedron so (A-D).n >= 0
+	S				= sign(sum(DA.*n, 1));
+	n				= bsxfun(@times, S, n);
+	
+	%% equation of plane: x.n + d = 0
+	d				= -sum(n.*A, 1);
+	
+	%% determine linear constraints
+	% each equation of plane has been chosen so that
+	% n.x + d = 0
+	% and if inside
+	% n.x + d > 0
+	% since n points outwards
+	A_hull		= n';
+	b_hull		= -d';
+end
\ No newline at end of file

calibration/bulk_marker_search.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% bulk_marker_search.m
+% find markers in images of calibration plates
+% then perform a 2D calibration to dewarp the calibration plate image
+
+addpath([pwd '/../mex/minmax/']);
+
+%% input parameters
+
+grid_dx				= 10;		% real units, mm
+grid_dy				= 10;
+invert_image		= 1;		% invert image for black dots on white
+blur_image			= 1;		% gaussian blur image
+normalise_image	= 1;
+xlim_zoom			= 1024*2/4 + [-512 512]/1;
+ylim_zoom			= 1024*2/4 + [-512 512]/1;
+
+output_dir			=		'D:/bigtank-jul-2013/CAL-20-07-13/NEWSHEET/';
+input_file_list   = {	'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-01/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-02/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-03/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-04/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-05/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-06/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-07/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-08/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-09/PLATE/CAM1_000001.tif', ...
+								'D:/bigtank-jul-2013/CAL-20-07-13/SHEET-10/PLATE/CAM1_000001.tif'};
+plate_origin		= [-24	0
+							-20	0
+							-16	0
+							-12	0
+							-8	0
+							-4	0
+							0	0
+							4	0
+							8	0
+							12	0]';
+camera_index		= 1;
+dot_half_window   = [24 24];
+calib_idx_offset	= 0;
+origin_pos			= [0 0];
+
+camera_clim			= [0 1];
+
+%% generate calibration for each image
+
+nCalibrations		= length(input_file_list);
+
+calib_idx			= 1;
+
+while calib_idx <= nCalibrations
+	%% update user of status
+	input_file		= input_file_list{calib_idx};
+	output_file		= sprintf('%s/CAM%d-%02d.mat', output_dir, camera_index, calib_idx+calib_idx_offset);
+	fprintf('Image %02d of %02d:\n', calib_idx, nCalibrations);
+	
+	% check if calibration already exists
+	if exist(output_file, 'file')
+		sInput		= input('Calibration already exists - overwrite? y to overwrite: ', 's');
+		if ~strcmpi(sInput, 'y')
+			calib_idx = calib_idx + 1;
+			continue;
+		end
+	end
+	% check if image input exists
+	if ~exist(input_file, 'file')
+		fprintf(' - could not find image file\n');
+	end
+	
+	%% load image, request position
+	im					= imread(input_file)';
+	im					= double(im);
+	im					= fliplr(im);
+	Nx					= size(im, 1);
+	Ny					= size(im, 2);
+	
+	%% create filtered image for processing
+	im_filt			= im;
+	if invert_image
+		im_filt		= max(im(:))-im;
+	end
+	if blur_image
+		G				= fspecial('gaussian', [5 5]);
+		im_filt		= filter2(G, im_filt);
+	end
+	if normalise_image
+		[minv, maxv]	= minmaxfiltnd(single(im_filt), dot_half_window*2+1);
+		im_filt			= (im_filt - minv) ./ max(32, maxv-minv);
+	end
+	
+	%% display image
+	figure(gcf);
+	hold off;
+	imagesc(im_filt');
+	axis equal tight xy;
+	colormap gray;
+	set(gca,'Clim', camera_clim);	
+	
+	%% get center, right and top marker position
+	bDone				= false;
+	while ~bDone
+		%% get marker position from user
+		fprintf('Please identify the origin, right and top markers:\n');
+		xlim(xlim_zoom);
+		ylim(ylim_zoom);
+		[i_coord,j_coord] = ginput(3);
+		axis tight;
+		
+		%% find barycenter of centre marker
+		dot_img_i		= round(i_coord(1)) + (-dot_half_window(1) : dot_half_window(1));
+		dot_img_j		= round(j_coord(1)) + (-dot_half_window(2) : dot_half_window(2));
+		[imat,jmat]		= ndgrid(dot_img_i, dot_img_j);
+		dot_img			= im_filt(dot_img_i, dot_img_j);
+		ij_center(1)	= sum(dot_img(:) .* imat(:)) / sum(dot_img(:));
+		ij_center(2)	= sum(dot_img(:) .* jmat(:)) / sum(dot_img(:));
+		
+		ij_right			= [i_coord(2) j_coord(2)];
+		ij_top			= [i_coord(3) j_coord(3)];
+		
+		%% ask user if they want to keep it
+		hold on;
+		plot_markers = plot(	ij_center(1), ij_center(2), 'bs', ...
+									ij_right(1), ij_right(2), 'ro', ...
+									ij_top(1), ij_top(2), 'gd');
+		%legend('Origin', 'Right', 'Top', 'Location', 'SouthOutside');
+		
+		sDone			= input('Enter y if you would like to try again: ', 's');
+		bDone			= true;
+		if strcmpi(sDone,'y')
+			bDone		= false;
+		end
+		delete(plot_markers);
+	end	
+	
+	%% find marker positions
+	[ij_markers, xy_markers, onx_idx, ony_idx] = find_calibration_markers(im_filt, ij_center, ij_right, ij_top, dot_half_window);
+	on_x_axis	= false(1,size(ij_markers,2));
+	on_y_axis	= false(1,size(ij_markers,2));
+	on_x_axis(onx_idx) = true;
+	on_y_axis(ony_idx) = true;
+	xy_markers(1,:) = xy_markers(1,:) * grid_dx + origin_pos(1);
+	xy_markers(2,:) = xy_markers(2,:) * grid_dy + origin_pos(2);	
+	
+	%% find positions more accurately using barycentres
+	Nmarkers		= size(ij_markers,2);
+	for marker_idx = 1 : Nmarkers
+		dot_img_i	= round(ij_markers(1,marker_idx)) + (-dot_half_window(1) : dot_half_window(1));
+		dot_img_j	= round(ij_markers(2,marker_idx)) + (-dot_half_window(2) : dot_half_window(2));
+		dot_img_i	= dot_img_i(dot_img_i > 1 & dot_img_i < Nx);
+		dot_img_j	= dot_img_j(dot_img_j > 1 & dot_img_j < Ny);
+		dot_img		= im_filt(dot_img_i, dot_img_j);
+		[imat,jmat]	= ndgrid(dot_img_i, dot_img_j);
+		ij_markers(1,marker_idx) = sum(dot_img(:).*imat(:)) / sum(dot_img(:));
+		ij_markers(2,marker_idx) = sum(dot_img(:).*jmat(:)) / sum(dot_img(:));
+	end
+	
+	%% throw away any markers too near edges
+	bad_markers		= ij_markers(1,:) < 10 | ij_markers(1,:) > (Nx-10) ...
+						| ij_markers(2,:) < 10 | ij_markers(2,:) > (Ny-10);
+	xy_markers		= xy_markers(:, ~bad_markers);
+	ij_markers		= ij_markers(:, ~bad_markers);
+	on_x_axis		= on_x_axis(~bad_markers);
+	on_y_axis		= on_y_axis(~bad_markers);
+	
+	%% plot x/y coords of each marker
+	n_markers = size(ij_markers, 2);
+		
+	for idx = 1 : n_markers
+		str		= sprintf('(%0.0f,%0.0f)', xy_markers(1,idx), xy_markers(2,idx));
+		text(	'Position', ij_markers(:,idx) + [-20; -20], ...
+				'String', str, ...
+				'FontSize', 8, ...
+				'Color', 'red');
+	end
+	
+	%% query user for markers to delete
+	bDone				= false;
+	while ~bDone		
+		figure(gcf);
+		plot_markers =	plot(	ij_markers(1,:), ij_markers(2,:), 'r+', ...
+									ij_markers(1,on_x_axis), ij_markers(2,on_x_axis), 'go', ...
+									ij_markers(1,on_y_axis), ij_markers(2,on_y_axis), 'gd', ...
+									ij_center(1), ij_center(2), 'bs');
+		%legend('Markers', 'X axis', 'Y axis', 'Center');
+		
+		bDone			= true;
+		sDelete		= input('Would you like to delete a marker? y to delete: ', 's');
+		if strcmp(sDelete, 'y')
+			bDone		= false;
+			fprintf('Click near the marker you''d like to delete\n');
+			[i_delete,j_delete] = ginput(1);
+			[idx, distance] = knnsearch(ij_markers', [i_delete,j_delete], 'K', 1);
+			if distance > 10
+				fprintf('You didn''t click anywhere near a marker!\n');				
+				continue;
+			end
+			keep			= (1 : size(ij_markers,2)) ~= idx;
+			ij_markers	= ij_markers(:, keep);
+			xy_markers	= xy_markers(:, keep);
+			on_x_axis	= on_x_axis(keep);
+			on_y_axis	= on_y_axis(keep);
+			
+			delete(plot_markers);
+		end
+	end
+	
+	%% create calibration and dewarp
+	xy_to_i_coeffs		= cubicfit_2d(xy_markers(1,:), xy_markers(2,:), ij_markers(1,:), 1);
+	xy_to_j_coeffs		= cubicfit_2d(xy_markers(1,:), xy_markers(2,:), ij_markers(2,:), 1);
+	ij_to_x_coeffs		= cubicfit_2d(ij_markers(1,:), ij_markers(2,:), xy_markers(1,:), 1);
+	ij_to_y_coeffs		= cubicfit_2d(ij_markers(1,:), ij_markers(2,:), xy_markers(2,:), 1);
+	
+	i_fit					= cubicmap_2d(xy_markers(1,:), xy_markers(2,:), xy_to_i_coeffs);
+	j_fit					= cubicmap_2d(xy_markers(1,:), xy_markers(2,:), xy_to_j_coeffs);
+	i_error				= ij_markers(1,:) - i_fit;
+	j_error				= ij_markers(2,:) - j_fit;
+	px_error				= sqrt(i_error.^2 + j_error.^2);
+	rms_error			= sqrt(mean(px_error.^2));
+	
+	fprintf('RMS fit error: %0.2f px\n', rms_error);
+
+	% work out x and y axes
+	[imat,jmat]			= ndgrid(1 : 32 : Nx, 1 : 32 : Ny);
+	xmat					= cubicmap_2d(imat, jmat, ij_to_x_coeffs);
+	ymat					= cubicmap_2d(imat, jmat, ij_to_y_coeffs);
+	x_lim					= fix([min(xmat(:)), max(xmat(:))]);
+	y_lim					= fix([min(ymat(:)), max(ymat(:))]);
+	x_axis				= linspace(x_lim(1), x_lim(2), Nx);
+	y_axis				= linspace(y_lim(1), y_lim(2), Ny);
+	i_axis				= 1 : Nx;
+	j_axis				= 1 : Ny;
+	% calculate dewarping
+	[xmat,ymat]			= ndgrid(x_axis, y_axis);
+	imat					= cubicmap_2d(xmat, ymat, xy_to_i_coeffs);
+	jmat					= cubicmap_2d(xmat, ymat, xy_to_j_coeffs);
+	im_xy					= interp2(i_axis, j_axis, im', imat, jmat, 'linear');
+	hold off;
+	imagesc(x_axis, y_axis, im_xy');
+	xlim(x_lim);
+	ylim(y_lim);
+	colormap gray;
+	axis xy equal tight;
+	hold on;
+	set(gca,'Clim', camera_clim);
+	for x = (round(x_lim(1)/grid_dx) : round(x_lim(end)/grid_dx)) * grid_dx
+		plot([x x], [y_axis(1) y_axis(end)], 'r-');
+	end
+	for y = (round(y_lim(1)/grid_dy) : round(y_lim(end)/grid_dy)) * grid_dy
+		plot([x_axis(1) x_axis(end)], [y y], 'r-');
+	end
+	
+	%% query user to save calibration or repeat
+	
+	sInput				= input('Would you like to repeat the calibration? y to repeat: ', 's');
+	if strcmpi(sInput, 'y')
+		continue; % continue without incrementing the counter
+	end
+	
+	%% save output
+	fprintf('Saving ... ');
+	fstruct				= struct('source_image', im, ...
+										'dewarped_image', im_xy, ...
+										'plate_pos',  plate_origin(:, calib_idx), ...
+										'xy_to_i_coeffs', xy_to_i_coeffs, 'xy_to_j_coeffs', xy_to_j_coeffs, ...
+										'ij_to_x_coeffs', ij_to_x_coeffs, 'ij_to_y_coeffs', ij_to_y_coeffs, ...
+										'i_axis', i_axis, 'j_axis', j_axis, ...
+										'x_axis', x_axis, 'y_axis', y_axis, 'x_lim', x_lim, 'y_lim', y_lim, ...
+										'grid_dx', grid_dx, 'grid_dy', grid_dy, ...
+										'ij_markers', ij_markers, 'xy_markers', xy_markers, ...
+										'ij_center', ij_center, 'ij_right', ij_right, 'ij_top', ij_top, ...
+										'camera_index', camera_index);
+	save(output_file, '-struct', 'fstruct');
+	fprintf('done\n');
+	calib_idx			= calib_idx + 1;
+end
\ No newline at end of file

calibration/bundle_calibration_fit.m

+function new_setup = bundle_calibration_fit(setup)
+	% perform a bundle calibration for a set of cameras and scene
+	% information provided in setup
+	
+	%% extract basic information
+	n_cameras	= setup.n_cameras;
+	n_views		= setup.n_views;
+	
+	%% initialise setup with a regular pinhole fit
+	fprintf('initial pinhole fit ... ');
+	s_cam_calib	= pinhole_camera_fit(setup);
+	for c = 1 : n_cameras
+		setup.camera(c).calib = s_cam_calib(c);
+	end
+	fprintf('done\n');
+	
+	%% initialise perturbation model
+	setup.omega			= eye(3);
+	setup.gl_plate_dx = zeros(3, n_views);
+	
+	%% fill arrays for least squares fitting
+	% uv_data should be n_total x 4 array of [u, v, cam, view]
+	% ij_data should be n_total x 2 array of [i, j] position of markers
+	n_total				= 0;
+	uv_data				= [];
+	ij_data				= [];
+	for c = 1 : n_cameras
+		camera			= setup.camera(c);
+		for v = 1 : n_views
+			%% extract marker position in plate coord sys and image
+			% xdata contains (u,v) coord of marker on plate
+			% as well as the index of the plate position and camera
+			% that the projection of this marker in image space corresponds to
+			uv_mark	= camera.view(v).uv_markers;
+			ij_mark	= camera.view(v).ij_markers;
+			n_mark	= size(uv_mark,2);
+			
+			uv_mark	= [uv_mark; c*ones(1,n_mark); v*ones(1,n_mark)];
+			uv_data	= [uv_data; uv_mark'];
+			ij_data  = [ij_data; ij_mark'];
+			n_total	= n_total + n_mark;
+		end
+	end
+	
+	%% least squares fitting operation
+	fprintf('doing bundle fit ...');
+	X0				= pack_model(setup);
+	NX				= length(X0);
+	opts			= optimset('MaxFunEvals', NX*10000, 'MaxIter', 10000, 'Display', 'on');
+	X				= lsqcurvefit(@(X,xdata) bundle_model(X,xdata,setup), X0, uv_data, ij_data, [], [], opts);
+	new_setup	= unpack_model(X, setup);
+	fprintf('done\n');
+end
+
+function ij_selected = bundle_model(perturb_vec, uv_mark, setup)
+	% given a perturbation (packed into perturbation vector)
+	% project markers with perturbed model
+	setup					= unpack_model(perturb_vec, setup); 
+	
+	%% perturbed position and orientation of plate
+	gl_plate_pos		= setup.gl_plate_pos + setup.gl_plate_dx;
+	pl_to_gl				= setup.omega * setup.pl_to_gl;
+	
+	%% project markers
+	n_cameras			= setup.n_cameras;
+	n_markers			= size(uv_mark, 1);
+	cam					= uv_mark(:, 3);
+	view					= uv_mark(:, 4);
+	% gl_mark and pl_mark are 3 x n_markers matrices
+	pl_mark				= [uv_mark(:, 1:2), zeros(n_markers,1)]'; 
+	gl_mark				= pl_to_gl*pl_mark + gl_plate_pos(:, view);
+	
+	% project markers in *every* camera
+	ij_mark				= zeros(2, n_markers, n_cameras);
+	for c = 1 : n_cameras
+		ij_mark(:,:,c)	= pinhole_model(gl_mark, setup.camera(c).calib);
+	end
+	
+	% return only the appropriate projections, according to uv_mark(:,3)
+	ij_selected			= zeros(n_markers, 2);
+	ij_selected(:,1)	= ij_mark(sub2ind([2 n_markers n_cameras], 1*ones(n_markers,1), (1:n_markers)', cam));
+	ij_selected(:,2)	= ij_mark(sub2ind([2 n_markers n_cameras], 2*ones(n_markers,1), (1:n_markers)', cam));
+end
+
+function X = pack_model(setup)
+	% function to pack a representation of the internal
+	% camera and scene model into a vector for least squares optimisation
+	% routine
+	
+	n_cameras		= setup.n_cameras;
+	n_views			= setup.n_views;
+	
+	%% camera model
+	X_cam				= zeros(15, n_cameras);
+	for c = 1 : n_cameras
+		calib			= setup.camera(c).calib;
+		G				= calib.G;
+		R				= calib.R;
+		r_vec			= vrrotmat2vec(R);
+		r_vec			= r_vec(1:3) * r_vec(4);
+		xc				= calib.xc;
+		kr				= calib.kr;
+		ic				= calib.ic;
+		
+		i_axis		= setup.camera(c).i_axis;
+		j_axis		= setup.camera(c).j_axis;
+		diag_px		= sqrt((i_axis(end)-i_axis(1))^2 + (j_axis(end)-j_axis(1))^2);
+		
+		X_cam(1,c)		= G(1,1);
+		X_cam(2,c)		= G(1,2);
+		X_cam(3,c)		= G(1,3) / diag_px;
+		X_cam(4,c)		= G(2,2);
+		X_cam(5,c)		= G(2,3) / diag_px;
+		X_cam(6:8,c)	= r_vec(:);
+		X_cam(9:11,c)	= xc(:);
+		% dewarping parameters are normalised to dimensionless quantities, 
+		% scaled by sensor size in px
+		X_cam(12:13,c)	= [kr(1)*diag_px^2; kr(2)*diag_px^4];
+		X_cam(14:15,c)	= ic(:) / diag_px;
+	end
+	
+	%% plate perturbation model
+	% plate perturbation consists of a displacement and rotation
+	X_plate		= setup.gl_plate_dx([1 3], :);
+	omega_vec	= vrrotmat2vec(setup.omega);
+	X_rot			= omega_vec(1:3) * omega_vec(4);
+	
+	X				= [X_cam(:); X_plate(:); X_rot(:)]';
+end
+
+function setup = unpack_model(X, setup)
+	% extract the perturbation model specified in X
+	% and update setup structure
+	n_views		= setup.n_views;
+	n_cameras	= setup.n_cameras;
+	
+	%% extract
+	off			= 1;
+	X_cam			= reshape(X(off : off-1 + n_cameras*15), [15 n_cameras]);
+	off			= off + n_cameras*15;
+	X_plate		= reshape(X(off : off-1 + n_views*2), [2 n_views]);
+	off			= off + n_views*2;
+	X_rot			= X(off : off + 2);
+	
+	%% update camera parameters
+	for c = 1 : n_cameras
+		i_axis	= setup.camera(c).i_axis;
+		j_axis	= setup.camera(c).j_axis;
+		diag_px	= sqrt((i_axis(end)-i_axis(1))^2 + (j_axis(end)-j_axis(1))^2);
+		
+		% intrinsic parameters
+		fx			= X_cam(1,c);
+		tau		= X_cam(2,c);
+		ox			= X_cam(3,c) * diag_px;
+		fy			= X_cam(4,c);
+		oy			= X_cam(5,c) * diag_px;
+		G			= [fx tau ox;
+						0  fy  oy;
+						0  0   1];
+		% rotation
+		r_vec		= X_cam(6:8,c);
+		R			= vrrotvec2mat([r_vec(:)/(eps+norm(r_vec)); norm(r_vec)]);
+		
+		% position
+		xc			= X_cam(9:11,c);
+		
+		% dewarping parameters are normalised to dimensionless quantities, 
+		% scaled by sensor size in px
+		kr			= [X_cam(12,c)/diag_px^2; X_cam(13,c)/diag_px^4]; % undo normalisation
+		ic			= X_cam(14:15,c) * diag_px;
+		
+		% full projection model
+		P			= G*R*[eye(3), -xc(:)];
+		
+		calib		= struct('P', P, 'G', G, 'R', R, 'xc', xc, 'kr', kr, 'ic', ic);
+		setup.camera(c).calib = calib;
+	end
+	
+	%% update plate position parameters
+	omega_vec	= [X_rot(:)/(eps+norm(X_rot)); norm(X_rot)];
+	setup.omega = vrrotvec2mat(omega_vec);
+	setup.gl_plate_dx = [X_plate(1,:); zeros(1,n_views); X_plate(2,:)]; 
+end
+
+

calibration/calcmask.m

+function mask = calcmask(camset, c0)
+	% mask = calcmask(camset, c)
+	%
+	% for each pixel in camera c of camset, determine the number
+	% of other cameras its epipolar ray is visible in
+	
+	%% get things
+	n_cameras= length(camset);
+	cam0		= camset(c0);
+	Ni			= length(cam0.i_axis);
+	Nj			= length(cam0.j_axis);
+	Npx		= Ni*Nj;
+	
+	%% dewarp
+	[im,jm]	= ndgrid(cam0.i_axis, cam0.j_axis);
+	ui_pos	= inverse_radial_distortion([im(:),jm(:)]', cam0.calib.ic, cam0.calib.kr);
+	
+	%% get equation of ray
+	% we write the equation of ray as
+	% x = x0 + s*e_s
+	ytilde	= [ui_pos; ones(1,Npx)];
+	GR			= cam0.calib.G * cam0.calib.R;
+	e_ray		= GR \ ytilde;
+	x0_ray	= cam0.calib.xc;
+	
+	%% determine mask
+	mask				= zeros(Ni, Nj);
+	for c1 = 1 : n_cameras
+		%% skip if same camera
+		if c0 == c1
+			continue;
+		end
+		
+		%% find linear inequality for bounds test
+		% the test for whether a point y is within the bounds of image space
+		% of this camera is described as an inequality Ay <= b. If satisfied,
+		% the point is within bounds, if not, it is outside bounds. we
+		% construct this inequality based on borders of image in undistorted
+		% image space
+		cam1			= camset(c1);
+		i_axis		= cam1.i_axis(:);
+		j_axis		= cam1.j_axis(:);
+		Ni1				= length(i_axis);
+		Nj1			= length(j_axis);
+		ir				= [1 : floor(Ni1/4) : Ni1, Ni1];
+		jr				= [1 : floor(Nj1/4) : Nj1, Nj1];
+		i_corners	= [i_axis(ir([1 1 1 1 1]));
+							i_axis(ir             );
+							i_axis(ir([5 5 5 5 5]));
+							i_axis(ir(end:-1:1   ))];
+		j_corners	= [j_axis(jr             );
+							j_axis(jr([5 5 5 5 5]));
+							j_axis(jr(end:-1:1   ));
+							j_axis(jr([1 1 1 1 1]))];
+		di_corners	= [i_corners'; j_corners'];
+		ui_corners	= inverse_radial_distortion(di_corners, cam1.calib.ic, cam1.calib.kr);
+		[A,b]			= bounding_polygon(ui_corners);
+
+		%% get equation of ray in image coordinates
+		% we want an equation of the form
+		% y = y0 + s * n
+		% that describes the equation of each ray from camera 0 in the
+		% (undistorted) image space of camera 1
+		
+		GR1					= cam1.calib.G * cam1.calib.R;
+		z0						= GR1(1:2,:)*(x0_ray - cam1.calib.xc);
+		zprime				= GR1(1:2,:)*e_ray;
+		lambda0				= GR1(  3,:)*(x0_ray - cam1.calib.xc);
+		lprime				= GR1(  3,:)*e_ray;
+		
+		y0						= bsxfun(@rdivide, zprime, lprime);
+		e_n					= bsxfun(@minus, z0, lambda0*y0);
+		e_n					= bsxfun(@rdivide, e_n, sqrt(sum(e_n.^2,1))); % normalise
+		
+		%% determine existence of solutions for inequality
+		% subsituting in y = y0 + s*e_n
+		% into our inequality Ay <= b
+		% yields the inequality
+		%		L*s <= R
+		% where L = A*e_n and R = b - A*y0
+		% if this inequality has solutions for any s, then there is at least
+		% one point on the ray which is visible in camera 1
+		
+		L						= A*e_n;
+		R						= bsxfun(@minus, b, A*y0);
+		gamma_lim			= R ./ L;
+		is_ub					= L >= 0;
+		is_lb					= L <  0;
+		
+		% find smallest upper bound
+		gamma_ub				= gamma_lim;
+		gamma_ub(is_lb)	= inf;	% ignore all those lower bounds
+		gamma_ub				= min(gamma_ub, [], 1);
+		
+		% find largest lower bound
+		gamma_lb				= gamma_lim;
+		gamma_lb(is_ub)	= -inf;	% ignore all the upper bounds
+		gamma_lb				= max(gamma_lb, [], 1);
+		
+		b_visible			= gamma_lb <= gamma_ub;
+		
+		%% add to sum
+		b_visible			= reshape(b_visible, [Ni Nj]);
+		mask					= mask + b_visible;
+	end
+end
+	
\ No newline at end of file

calibration/calibration_to_config.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% based on a camera calibration performed in MATLAB using John Lawson's
+% code, create a corresponding .cfg file for use with Haitao Xu's PTV code
+% 
+
+addpath([pwd '/../calibration-mpids/']);
+
+%% input parameters
+calib_dir					= '/data/cluster/160323/';
+calib_file_mat				= [calib_dir '/camera-model-160323.mat'];
+calib_file_cfg				= [calib_dir '/PTVSetup_160323.cfg'];
+points_file_pat			= [calib_dir '/calibpointspos.cam%d.dat'];
+
+pixel_pitch_mm				= [10E-3 10E-3];
+
+%% load calibration
+fs								= importdata(calib_file_mat);
+n_cameras					= fs.n_cameras;
+camera						= fs.camera;
+
+%% calculate Tsai camera calibration
+fprintf('Calculating Tsai calibration ... ');
+
+for c = 1 : n_cameras
+	%% load parameters
+	i_axis					= camera(c).i_axis;
+	j_axis					= camera(c).j_axis;
+	Ni							= length(i_axis);
+	Nj							= length(j_axis);
+	cparams					= struct('Npixh', Nj, 'Npixw', Ni, ...
+											'hpix', pixel_pitch_mm(1), ...
+											'wpix', pixel_pitch_mm(2));
+	
+	%% load markers and calculate residuals
+	gl_markers				= camera(c).calib.gl_markers;
+	ij_markers				= camera(c).calib.ij_markers;
+	ij_markers_proj		= pinhole_model(gl_markers, camera(c).calib, true);
+	model_res				= ij_markers - ij_markers_proj;
+	
+	%% generate Tsai calibration
+	% Tsai calibration uses a different coordinate system for image plane
+	% in our calibration, (i,j) corresponds to x,y coordinate, as measured
+	% from the bottom left corner. ij the Tsai calibration, (i,j)
+	% coordinates start from the top left corner and move down and across.
+	ij_markers_perm			= [ij_markers(1,:);
+										Nj-1 - ij_markers(2,:)];
+	
+	[tsai_calib, tsai_res]	= calib_Tsai(ij_markers_perm', gl_markers', cparams);
+	tsai_res						= tsai_res';
+	camera(c).tsai_calib		= tsai_calib;
+	
+	%% save calibpointspos file
+	points_file				= sprintf(points_file_pat, c - 1);
+	fid						= fopen(points_file, 'w');
+	fprintf(fid, '# pimg_x (pix)\t pimg_y (pix) \t x (mm) \t y (mm) \t z (mm) \t Np \t sum(x) \t sum(y) \t sum(I) \n');
+	fprintf(fid, '%12.7f\t%12.7f\t%12.7f\t%12.7f\t%12.7f\t0\n', [ij_markers_perm; gl_markers]);
+	fclose(fid);
+	
+	%% generate statistics of error
+	model_rms				= rms(model_res, 2);
+	tsai_rms					= rms(tsai_res, 2);
+	
+	%% report on error
+	fprintf('Camera %d\n', c);
+	fprintf('   Regular:   %0.3f    %0.3f\n', model_rms);
+	fprintf('   Tsai:      %0.3f    %0.3f\n', tsai_rms);
+end
+fprintf('done\n');
+
+%% write config file
+fprintf('Saving to file ... ');
+
+% Now write the configuration file
+fid = fopen(calib_file_cfg, 'w');
+fprintf(fid, '# PTV experiment configuration file\n');
+fprintf(fid, '\n %d\t# ncams\n\n', n_cameras);
+for c = 1 : n_cameras
+	%% load info
+	tsai_calib				= camera(c).tsai_calib;
+	i_axis					= camera(c).i_axis;
+	j_axis					= camera(c).j_axis;
+	Ni							= length(i_axis);
+	Nj							= length(j_axis);
+	
+	%% write to file
+	fprintf(fid, '######## cam %d ########\n\n', c - 1);
+	fprintf(fid, '%d\t\t\t# Npix_x\n', Ni);
+	fprintf(fid, '%d\t\t\t# Npix_y\n', Nj);
+	fprintf(fid, '%11.8f\t\t\t# pixsize_x (mm)\n', pixel_pitch_mm(1));
+	fprintf(fid, '%11.8f\t\t# pixsize_y (mm)\n', pixel_pitch_mm(1));
+	fprintf(fid, '%12.8f\t\t# effective focal length (mm)\n', tsai_calib.f_eff);
+	% Note the sign change for k1, because its meaning is different in calib_Tsai and the stereomatching code
+	fprintf(fid, '%15.8e\t# radial distortion k1 (1/pixel)\n', -tsai_calib.k1);	
+	fprintf(fid, '%15.8e\t# tangential distortion p1 (1/pixel)\n', tsai_calib.p1);
+	fprintf(fid, '%15.8e\t# tangential distortion p2 (1/pixel)\n', tsai_calib.p2);
+	fprintf(fid, '0.0\t\t# x0\n');
+	fprintf(fid, '0.0\t\t# y0\n');
+    % Parameters for camera movement correction
+	fprintf(fid, '0.0\t\t# x0_offset (pixel)\n');
+	fprintf(fid, '0.0\t\t# y0_offset (pixel)\n');
+	fprintf(fid, '1.0\t\t# x_rot (cos(theta))\n');
+	fprintf(fid, '0.0\t\t# y_rot (sin(theta))\n');
+	fprintf(fid, '# rotation matrix R\n');
+	fprintf(fid, '%12.8f\t%12.8f\t%12.8f\n', (tsai_calib.R)');
+	fprintf(fid, '# translation vector T\n');
+	fprintf(fid, '%15.8f\n', tsai_calib.T);
+	fprintf(fid, '# inverse rotation matrix Rinv\n');
+	fprintf(fid, '%12.8f\t%12.8f\t%12.8f\n', (tsai_calib.Rinv)');
+	fprintf(fid, '# inverse translation vector Tinv\n');
+	fprintf(fid, '%15.8f\n', tsai_calib.Tinv);
+	fprintf(fid, '# rms distance between particle centers found on image plane and their projections\n');
+	fprintf(fid, '# %15.8f\t\t# err_x\n', tsai_calib.err_x);
+	fprintf(fid, '# %15.8f\t\t# err_y\n', tsai_calib.err_y);
+	fprintf(fid, '# %15.8f\t\t# err_t\n', tsai_calib.err_t);
+	fprintf(fid, '\n');
+end
+% laser beam parameters
+fprintf(fid, '##### laser beam #####\n');
+fprintf(fid, '\n');
+fprintf(fid, '0\t\t# finite volume illumination\n');
+fprintf(fid, '-50\t\t# illum_xmin\n');
+fprintf(fid, '+50\t\t# illum_xmax\n');
+fprintf(fid, '-50\t\t# illum_ymin\n');
+fprintf(fid, '+50\t\t# illum_ymax\n');
+fprintf(fid, '-50\t\t# illum_zmin\n');
+fprintf(fid, '+50\t\t# illum_zmax\n');
+fprintf(fid, '\n');
+% 3D matching parameters
+fprintf(fid, '#### thresholds for 3D matching ####\n');
+fprintf(fid, '\n');
+fprintf(fid, '3.0\t\t# mindist_pix (pixel)\n');
+fprintf(fid, '1.0\t\t# maxdist_3D (mm)\n');
+fclose(fid);
+
+fprintf('done\n');
\ No newline at end of file

calibration/calpts2mat.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% load a text file from Jan/Holly's calibration 
+% containing indices/image coordinates of calibration points
+% and convert this to the common .MAT file format
+
+addpath([pwd '/pinhole/']);
+
+%% input parameters
+calib_dir			= '/nfs/scratch21/lawson/zugspitze/calib/';
+mat_file_pat		= '%s/CAM%d-%02d.mat';
+calpts_file_pat	= '%s/CalibPts%d.txt';
+
+camera_index		= 3;
+camera_id			= camera_index - 1;
+grid_dx				= 0.5;		% real units, mm
+grid_dy				= 0.5;	
+
+img_width			= 1280;
+img_height			= 800;
+i_axis				= 0 : 1 : img_width  - 1;
+j_axis				= 0 : 1 : img_height - 1;
+
+DO_DEBUG_PLOT		= true;
+
+%% load calibration points file
+fname					= sprintf(calpts_file_pat, calib_dir, camera_id);
+C						= textread(fname);
+gl_markers			= C(:, 1:3)';
+ij_markers			= C(:, [5 4])';
+
+%% get plate pos from z
+plate_pos			= unique(gl_markers(3,:))';
+n_views				= length(plate_pos);
+plate_pos			= [zeros(n_views,2), plate_pos];
+
+%% iterate over planes
+for calib_idx = 1 : n_views
+	%% load subset belonging to this plane
+	rng				= gl_markers(3,:) == plate_pos(calib_idx, 3);
+	xy_mark			= gl_markers(1:2, rng);
+	ij_mark			= ij_markers(:, rng);
+	gl_plate_pos	= plate_pos(calib_idx,:);
+	
+	%% fit pinhole
+	[P2D, ij_res]	= p2d_fit(xy_mark, ij_mark);
+	ij_rms			= rms(ij_res, 2);
+	
+	%% axes and limits
+	x_lim						= [min(xy_mark(1,:)), max(xy_mark(1,:))];
+	y_lim						= [min(xy_mark(2,:)), max(xy_mark(2,:))];
+	Nx							= img_width;
+	Ny							= img_height;
+	x_axis					= linspace(x_lim(1), x_lim(2), Nx);
+	y_axis					= linspace(y_lim(1), y_lim(2), Ny);
+	
+	%% plot
+	if DO_DEBUG_PLOT
+		figure(1);
+		clf;
+		plot3(xy_mark(1,:), xy_mark(2,:), ij_mark(1,:), 'b.');
+		hold on;
+		plot3(xy_mark(1,:), xy_mark(2,:), ij_mark(2,:), 'r.');
+	end
+	
+	%% save	
+	output_file			= sprintf(mat_file_pat, calib_dir, camera_index, calib_idx);
+	fprintf('Saving to %s... ', output_file);
+	fstruct				= struct('plate_pos',  gl_plate_pos, ...
+										'P', P2D, ...
+										'i_axis', i_axis, 'j_axis', j_axis, ...
+										'x_axis', x_axis, 'y_axis', y_axis, 'x_lim', x_lim, 'y_lim', y_lim, ...
+										'grid_dx', grid_dx, 'grid_dy', grid_dy, ...
+										'ij_markers', ij_mark, 'xy_markers', xy_mark, ...
+										'camera_index', camera_index, ...
+										'id', camera_id);
+	save(output_file, '-struct', 'fstruct');
+	fprintf('done\n');
+	
+	%% report
+	fprintf('2D fit plane %d:\n', calib_idx);
+	fprintf(' RMS:   %0.3f %0.3f px\n', ij_rms);
+end
\ No newline at end of file

calibration/camera_calibration.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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					= '/nfs/scratch23/lawson/K62/170203/calib/';
+cam_calib_file_pat		= [calib_dir '/CAM%d-%02d.mat'];
+cam_model_file				= [calib_dir '/camera-model-original.mat'];
+
+cam_index_range			= [1 2];
+cam_view_range				= 1 : 7;
+n_views						= length(cam_view_range);
+n_cameras					= length(cam_index_range);
+n_iterations				= 2;
+
+DO_DEBUG_PLOT				= 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);
+
+for cam_idx = 1 : n_cameras
+	camera					= emptyCamera;
+	
+	for view_idx = 1 : n_views
+		calib_file			= sprintf(cam_calib_file_pat, 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, 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');
+
+%% pinhole fit before bundle calibration
+fprintf('fitting camera model to setup before bundle calibration:\n');
+[s_old_calib,res_before]= pinhole_camera_fit(setup);
+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);
+	
+	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);
+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);
+
+% add to setup
+new_setup.gl_origin		= gl_origin;
+new_setup.r_sphere		= r_sphere;
+new_setup.r_sphere_lim	= r_sphere_lim;
+
+%% 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 DO_DEBUG_PLOT
+	%% colours for different views
+	cmap				= jet;
+	cvec_map			= linspace(0,1,size(cmap,1));
+	cvec_int			= linspace(0,1,n_views);
+	marker_colour	= zeros(n_views,3);
+	for rgb = 1 : 3
+		marker_colour(:,rgb) = interp1(cvec_map, cmap(:,rgb), cvec_int, 'linear');
+	end
+	marker_colour(1,:)	= [1 0 0];
+	marker_colour(end,:) = [0 0 1];
+
+	%% 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
\ No newline at end of file

calibration/check_calibration.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% check calibration by loading and dewarping particle images
+% 
+
+addpath([pwd '/../reconstruction/']);
+
+%% input parameters
+
+omega_y_mrad					= -40;
+
+% image to dewarp and plot
+cam_index						= 2;
+figure_index					= 100*cam_index;
+sheet_index_range				= 20;%[11:14];
+volume_index					= 1;
+series_index					= 1;
+
+n_sheets							= length(sheet_index_range);
+
+% calibration
+calibration_file				= 'F:/bigtank-jul-2013/CAL-06-08-13/scanning-setup.mat';
+
+% image processing options
+DO_IMAGE_FLIPUPDOWN			= 0;
+DO_IMAGE_FLIPLEFTRIGHT		= 0;
+DO_DARK_IMAGE					= 1;
+image_clim						= [0 4];
+
+% path of files to reconstruct
+source_img_dir					= 'F:/bigtank-jul-2013/MEAS-06-08-13/SCAN-05/';
+dark_img_dir					= 'F:/bigtank-jul-2013/DARK/MEAS-06-08-13/SCAN-05/';
+source_img_file_pat			= 'CAMID%d-series-%02d-volume-%02d-frame-%02d.raw12';
+dark_img_file_pat				= 'CAMID%d-frame-%02d.raw12';
+
+source_file_name_fun			= @(camid,series,volume,frame) sprintf(['%s/' source_img_file_pat], source_img_dir, camid, series, volume, frame);
+dark_file_name_fun			= @(camid,frame) sprintf(['%s/' dark_img_file_pat], dark_img_dir, camid, frame);
+
+%% load the calibration data
+fprintf('------------------------------------------------------------------\n');
+fprintf('CALIBRATION:\n');
+
+fstruct							= importdata(calibration_file);
+% camera id
+camera_id						= fstruct.camera_id(cam_index);
+% coordinate system and axes
+volume_x							= fstruct.X;
+volume_y							= fstruct.Y;
+volume_z							= fstruct.Z;
+volume_Lx						= volume_x(end) - volume_x(1);
+volume_Ly						= volume_y(end) - volume_y(1);
+volume_Lz						= volume_z(end) - volume_z(1);
+volume_Nx						= length(volume_x);
+volume_Ny						= length(volume_y);
+volume_Nz						= length(volume_z);
+% camera mapping functions
+sCamCalib						= fstruct.camera_calibration(cam_index);
+re_plane_coeffs				= fstruct.re_plane_coeffs;
+% image coordinate system
+im_i								= fstruct.i;
+im_j								= fstruct.j;
+
+
+%% modify plane normal
+
+re_plane_normal				= re_plane_coeffs(:, 1:3);
+Omega								= vrrotvec2mat([0 1 0 omega_y_mrad/1000]);
+re_plane_normal				= (Omega*re_plane_normal')';
+re_plane_coeffs(:,1:3)		= re_plane_normal;
+
+%% read the images from file
+fprintf(' --- loading images ... ');
+source_file_names				= arrayfun(source_file_name_fun, ones(1,n_sheets)*camera_id, ...
+												  ones(1,n_sheets)*series_index, ...
+												  ones(1,n_sheets)*volume_index, ...
+												  sheet_index_range, 'UniformOutput', false);
+[imgvol, fail]					= load_image_array(source_file_names, DO_IMAGE_FLIPUPDOWN, DO_IMAGE_FLIPLEFTRIGHT);
+
+if DO_DARK_IMAGE
+	dark_file_names			= arrayfun(	dark_file_name_fun, ones(1, n_sheets)*camera_id, ...
+													sheet_index_range, 'UniformOutput', false);
+	darkvol						= load_image_array(dark_file_names, DO_IMAGE_FLIPUPDOWN, DO_IMAGE_FLIPLEFTRIGHT);
+	% subtract dark images
+	imgvol						= max(imgvol - darkvol, 0);
+end
+
+fprintf('done\n');
+
+%% dewarp each image
+% back-projection onto planes with regular x and y
+% (z coordinate determined by plane of laser sheet)
+imgvol_dewarped				= zeros(volume_Nx, volume_Ny, n_sheets);
+
+[xmat, ymat]					= ndgrid(volume_x, volume_y);
+
+for it = 1 : n_sheets
+	%% calculate z coords on this plane and get image coords
+	sheet_index					= sheet_index_range(it);
+	M								= re_plane_coeffs(sheet_index, :);
+	zmat							= -(M(4) + M(1)*xmat + M(2)*ymat) / M(3);
+	xyzmat						= [xmat(:) ymat(:) zmat(:)]';
+	ij								= pinhole_model(xyzmat, sCamCalib);
+	
+	%% load image and interpolate
+	im								= imgvol(:, :, it);
+	im_int						= interp2(im_i, im_j, im', ij(1,:), ij(2,:), 'linear', nan);
+	im_int						= reshape(im_int, [volume_Nx volume_Ny]);
+	imgvol_dewarped(:,:,it)	= im_int;
+end
+
+%% display
+
+for it = 1 : n_sheets
+	figure(figure_index + sheet_index_range(it));
+	hold off;
+	im		= imgvol_dewarped(:,:,it);
+	imagesc(volume_x, volume_y, im' / rms(im(:)));
+	axis equal tight xy;
+	set(gca,'clim',image_clim);
+end

calibration/combine_calibrations.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% combine_c.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
+
+calib_dir					= 'D:/bigtank-jul-2013/CAL-20-07-13/NEWSHEET/';
+cam_model_file				= [calib_dir '/camera-model-280415.mat'];
+sheet_model_file			= [calib_dir '/sheet-model-280415.mat'];
+setup_file					= [calib_dir '/scanning-setup-280415.mat'];
+poly_order					= 5;
+
+%% load camera calibration
+
+fstruct					= importdata(cam_model_file);
+n_cameras				= fstruct.n_cameras;
+camera_setup			= fstruct.camera;
+im_i						= camera_setup(1).i_axis;
+im_j						= camera_setup(1).j_axis;
+
+%% load laser sheet calibration
+
+fstruct					= importdata(sheet_model_file);
+n_sheets					= fstruct.n_sheets;
+gl_eZ_mat				= fstruct.gl_sheet_normal;
+gl_xs_mat				= fstruct.gl_sheet_pos;
+gl_on_sheet				= fstruct.gl_on_sheet;
+w_vec						= fstruct.sheet_width;
+
+%% define orientation of [X,Y,Z] coordinate system
+
+% orentation of reconstruction coordinate system in global system
+gl_eZ						= mean(gl_eZ_mat, 1);
+gl_eX						= cross([0 1 0]', gl_eZ);
+gl_eX						= gl_eX / norm(gl_eX);
+gl_eY						= cross(gl_eZ, gl_eX);
+
+% transfer matrices for gl->re and re->gl orientation 
+re_to_gl					= [gl_eX(:), gl_eY(:), gl_eZ(:)];
+gl_to_re					= re_to_gl^-1;
+
+% origin of reconstruction coordinate system
+gl_X0						= [0 0 0]';						% origin in gl coordinates
+re_X0						= gl_to_re * gl_X0;			% origin in re
+re_eZ_mat				= (gl_to_re * gl_eZ_mat')';
+re_xs_mat				= (gl_to_re * (gl_xs_mat - ones(n_sheets,1)*gl_X0(:)')')';
+
+%% calculate the plane coefficients
+% these are a set of 4 coefficients per plane, M(1:4)
+% all points [X,Y,Z] on this plane satisfy XM(1) + YM(2) + ZM(3) + M(4) = 0
+
+re_plane_coeffs			= zeros(n_sheets, 4);
+
+for sheet_idx = 1:n_sheets
+	% get equation of plane dot(M,X) = 1
+	gl_sheet_xs				= gl_xs_mat(sheet_idx, :)';
+	gl_sheet_en				= gl_eZ_mat(sheet_idx, :)';
+	re_sheet_xs				= gl_to_re * (gl_sheet_xs - gl_X0);
+	re_sheet_en				= gl_to_re * gl_sheet_en;
+	re_sheet_en				= re_sheet_en / norm(re_sheet_en);
+	% save to matrix
+	re_plane_coeffs(sheet_idx,1:3) = re_sheet_en;
+	re_plane_coeffs(sheet_idx,4)   = -dot(re_sheet_xs, re_sheet_en);
+end
+
+% smooth out errors by replacing measured value with fitted value
+k								= 0 : n_sheets - 1;
+re_plane_coeffs_o			= re_plane_coeffs;
+for i = 1:4	
+	p							= polyfit(k, re_plane_coeffs(:,i)', poly_order);
+	re_plane_coeffs(:,i)	= polyval(p, k);
+end
+% fix coefficients so M(1:3) is a unit vector
+for i = 1 : n_sheets
+	en							= re_plane_coeffs(i,1:3);
+	d							= re_plane_coeffs(i,4);
+	re_plane_coeffs(i,:) = [en d] / norm(en);
+end
+
+sheet_spacing				= polyfit(k, re_plane_coeffs(:,4)', 1);
+sheet_spacing				= abs(sheet_spacing(1));
+
+%% convert camera calibration to reconstruction coordinate system
+% incorporates shift of origin and rotation
+
+for cam_idx = 1 : n_cameras
+	gl_pinhole_model		= camera_setup(cam_idx).calib;
+	gl_G						= gl_pinhole_model.G;
+	gl_R						= gl_pinhole_model.R;
+	gl_xc						= gl_pinhole_model.xc;
+	re_R						= gl_R * re_to_gl;
+	re_xc						= gl_to_re * (gl_xc(:) - gl_X0(:));
+	re_P						= gl_G * re_R * [eye(3) -re_xc];
+	
+	re_pinhole_model		= gl_pinhole_model;
+	re_pinhole_model.P	= re_P;
+	re_pinhole_model.R	= re_R;
+	re_pinhole_model.xc	= re_xc;
+	camera_setup(cam_idx).calib	= re_pinhole_model;
+	camera_setup(cam_idx).gl_calib= gl_pinhole_model;
+end
+
+%% calculate axes for reconstruction volume
+% with knowledge of each camera's pinhole model
+% and the laser sheet setup
+% we can define axes for the reconstruction volume
+
+X_lim_mat			= zeros(n_cameras, 2);
+Y_lim_mat			= zeros(n_cameras, 2);
+Z_lim_mat			= zeros(n_cameras, 2);
+
+ilim					= [im_i(3) im_i(end-2)];
+jlim					= [im_j(3) im_j(end-2)];
+
+% report on what we are about to do
+fprintf('------------------------------------------------------------------\n');
+fprintf('CAMERA CALIBRATION\n');
+fprintf('Number of cameras:        %d\n', n_cameras);
+
+for cam_idx = 1 : n_cameras
+	%% find the extent of the visible volume
+	% the largest possible volume, bound by first and last laser sheets
+	% which projects onto the space between ilim(1) < i < ilim(2)
+	% and jlim(1) < j < lim(2)
+	M1						= re_plane_coeffs(1, :);
+	Mend					= re_plane_coeffs(end, :);
+	camera_calib		= camera_setup(cam_idx).calib;
+	P						= camera_calib.P;
+	[XLim,YLim,ZLim]	= find_largest_volume(P, ilim, jlim, M1, Mend);
+	% add a little extra bit of space in reconstruction around edges in z
+	% direction
+	ZLim					= ZLim + sheet_spacing*[-2 2]';
+	% save
+	X_lim_mat(cam_idx,:)= XLim;
+	Y_lim_mat(cam_idx,:)= YLim;
+	Z_lim_mat(cam_idx,:)= ZLim;
+	
+	%% report for this camera
+	fprintf('Camera %d, ID = %d\n', cam_idx, camera_setup(cam_idx).id);
+	fprintf('X limits:         %0.1f to %0.1f\n', XLim(1), XLim(2));
+	fprintf('Y limits:         %0.1f to %0.1f\n', YLim(1), YLim(2));
+	fprintf('Z limits:         %0.1f to %0.1f\n', ZLim(1), ZLim(2));
+	fprintf('Camera position:  %0.2f %0.2f %0.2f\n', camera_calib.xc);
+	fprintf('Internal camera parameters:\n');
+	disp(camera_calib.G);
+end
+
+%% define a coordinate system visible to all cameras
+X_lim				= [max(X_lim_mat(:,1)) min(X_lim_mat(:,2))];
+Y_lim				= [max(Y_lim_mat(:,1)) min(Y_lim_mat(:,2))];
+Z_lim				= [max(Z_lim_mat(:,1)) min(Z_lim_mat(:,2))];
+% fix resolution
+re_dY				= (Y_lim(2) - Y_lim(1)) / (jlim(2) - jlim(1));
+re_dX				= re_dY;
+re_dZ				= re_dX;
+re_X				= X_lim(1) : re_dX : X_lim(2);
+re_Y				= Y_lim(1) : re_dY : Y_lim(2);
+re_Z				= Z_lim(1) : re_dZ : Z_lim(2);
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('Combined calibration:\n');
+fprintf('X limits:         %0.1f to %0.1f (%d vox)\n', X_lim(1), X_lim(2), length(re_X));
+fprintf('Y limits:         %0.1f to %0.1f (%d vox)\n', Y_lim(1), Y_lim(2), length(re_Y));
+fprintf('Z limits          %0.1f to %0.1f (%d vox)\n', Z_lim(1), Z_lim(2), length(re_Z));
+fprintf('voxel size:       %0.2f x %0.2f x %0.2f\n', re_dX, re_dY, re_dZ);
+
+%% save to file
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('Multiple camera calibration complete\n');
+fprintf('Saving to file ... ');
+
+fstruct			= struct('n_sheets', n_sheets, ...
+								'gl_sheet_normal', gl_eZ_mat, 're_sheet_normal', re_eZ_mat, ...
+								'gl_sheet_pos', gl_xs_mat, 're_sheet_pos', re_xs_mat, ...
+								're_plane_coeffs', re_plane_coeffs, ...
+								'sheet_width', w_vec, ...
+								'gl_X0', gl_X0, ...
+								'gl_to_re', gl_to_re, ...
+								'i', im_i, 'j', im_j, ...
+								'X', re_X, 'Y', re_Y, 'Z', re_Z, ...								
+								'n_cameras', n_cameras, ...
+								'camera_calibration', [camera_setup.calib], ...
+								'camera_id', [camera_setup.id]); 
+save(setup_file, '-struct', 'fstruct');
+fprintf('done\n');
+
+%% plot residuals of fit for sheet coefficients
+figure(1);
+clf;
+plot(k, re_plane_coeffs_o(:,1) - re_plane_coeffs(:,1), 'r.', ...
+	  k, re_plane_coeffs_o(:,2) - re_plane_coeffs(:,2), 'g.', ...
+	  k, re_plane_coeffs_o(:,3) - re_plane_coeffs(:,3), 'b.');
+xlabel('sheet index');
+ylabel('coefficient residual (unit vectors)');
+title('residuals for polynomial fit of laser sheet normal');
+
+figure(2);
+plot(k, (re_plane_coeffs_o(:,4) - re_plane_coeffs(:,4))/re_dX, 'k.', ...
+	  [0 n_sheets-1], [1 1]*+0.01 * sheet_spacing / re_dX, 'k--', ...
+	  [0 n_sheets-1], [1 1]*-0.01 * sheet_spacing / re_dX, 'k--');
+ylim(sheet_spacing * [-1 1]*0.1 / re_dX);
+xlabel('sheet index');
+ylabel('coefficient residual (normal distance) / vx');
+title('residuals for polynomial fit of laser sheet position');
+
+%% plot projection of reconstruction volume in each view
+
+figure(9);
+clf;
+for cam_idx = 1 : n_cameras
+	%% create figure
+	subplot(1,n_cameras,cam_idx);
+	cla;
+	
+	%% plot corners
+	camera_calib		= camera_setup(cam_idx).calib;
+	corners_xyz			= [X_lim([1 1 1 1 2 2 2 2]);
+								Y_lim([1 1 2 2 1 1 2 2]);
+								Z_lim([1 2 1 2 1 2 1 2])];
+	corners_ij			= pinhole_model(corners_xyz, camera_calib);
+	plot(	corners_ij(1,[1 2 3 4 1]), corners_ij(2, [1 2 3 4 1]), 'k.-', ...
+			corners_ij(1,[5 6 7 8 5]), corners_ij(2, [5 6 7 8 5]), 'k.-', ...
+			corners_ij(1,[1 3 7 5 1]), corners_ij(2, [1 3 7 5 1]), 'k.-', ...
+			corners_ij(1,[2 4 8 6 2]), corners_ij(2, [2 4 8 6 2]), 'k.-');
+	% plot without pinhole model
+	hold on
+	camera_calib.kr = [0 0];
+	camera_calib.ic = [512 512];
+	corners_ij			= pinhole_model(corners_xyz, camera_calib);
+	plot(	corners_ij(1,[1 2 3 4 1]), corners_ij(2, [1 2 3 4 1]), 'r.-', ...
+			corners_ij(1,[5 6 7 8 5]), corners_ij(2, [5 6 7 8 5]), 'r.-', ...
+			corners_ij(1,[1 3 7 5 1]), corners_ij(2, [1 3 7 5 1]), 'r.-', ...
+			corners_ij(1,[2 4 8 6 2]), corners_ij(2, [2 4 8 6 2]), 'r.-');
+	%% make pretty
+	axis equal tight xy;
+	xlim(im_i([1 end]));
+	ylim(im_j([1 end]));
+	xlabel('i');
+	ylabel('j');
+	title(sprintf('Camera %d, ID %d', cam_idx, camera_setup(cam_idx).id));
+end
+
+
+%% plot in reconstruction coordinate system
+
+figure(10);
+clf;
+hold off;
+
+% plot planes 
+M1				= re_plane_coeffs(1,:);
+Mend			= re_plane_coeffs(end,:);
+x_corners	= [X_lim(1) X_lim(1) X_lim(2) X_lim(2) X_lim(1)];
+y_corners	= [Y_lim(1) Y_lim(2) Y_lim(2) Y_lim(1) Y_lim(1)];
+% first plane
+z_corners	= -(M1(1)*x_corners + M1(2)*y_corners + M1(4)) / M1(3);
+P1				= patch(x_corners, y_corners, z_corners, 'g');
+
+% last plane
+z_corners	= -(Mend(1)*x_corners + Mend(2)*y_corners + Mend(4)) / Mend(3);
+Pend			= patch(x_corners, y_corners, z_corners, 'g');
+set([P1 Pend], 'FaceAlpha', 0.5, 'edgealpha', 1);
+
+hold on;
+
+% plot viewing frustum of each camera
+ilim			= [im_i(1) im_i(end)];
+jlim			= [im_j(1) im_j(end)];
+i_corners	= [ilim(1) ilim(1) ilim(2) ilim(2)];
+j_corners	= [jlim(1) jlim(2) jlim(2) jlim(1)];
+ij_corners  = [i_corners; j_corners];
+
+str_colour	= {'r','k','b'};
+
+for cam_idx = 1 : n_cameras
+	calib		= camera_setup(cam_idx).calib;
+	Gc			= calib.G;
+	Rc			= calib.R;
+	xc			= calib.xc;
+	ic			= calib.ic;
+	kr			= calib.kr;
+	
+	% convert dewarped image coordinates to pinhole image coordinates 
+	ij_undist= inverse_radial_distortion(ij_corners, ic, kr);
+	for corner = 1 : 4
+		ytilde= [ij_undist(:, corner); 1];
+		e_ray = Rc^-1 * Gc^-1 * ytilde;
+		e_ray = e_ray / norm(e_ray);
+		
+		x_far = xc + e_ray * norm(xc) * 1.2;
+		x_near= xc;
+		
+		plot3([x_near(1) x_far(1)], ...
+				[x_near(2) x_far(2)], ...
+				[x_near(3) x_far(3)], '-', 'Color', str_colour{cam_idx});
+	end
+end
+
+axis equal;% tight xy;
+xlim([-100 100]);
+ylim([-100 100]);
+zlim([-100 100]);
\ No newline at end of file

calibration/combine_markers.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% combine_markers.m
+%
+% combine calibration images of the same markers from a single camera
+% 
+addpath([pwd '/../../piv2d/']);
+addpath([pwd '/pinhole/']);
+
+%% input parameters
+
+calibration_dir	= '/nfs/data/lawson/160715/calib/';
+calib_infile_pat	= 'CAM2-%02d.mat';
+calib_outfile_pat = 'C-CAM2-%02d.mat';
+calib_file_range  = 1 : 16;
+peak_mag_threshold= 0.5;
+
+marker_size			= [0.25 0.25];	% mm, interpreted as half-size
+marker_min_bright = 50;	% minimum brightness of marker image
+
+%% load calibration files
+
+x_lim					= [inf -inf];
+y_lim					= [inf -inf];
+n_calibrations		= length(calib_file_range);
+
+fprintf('------------------------------------------------------------------\n');
+fprintf('Loading %d calibration files ... ', n_calibrations);
+
+for file_idx = calib_file_range
+	file_name		= sprintf([calibration_dir '/' calib_infile_pat], file_idx);
+	fstruct			= importdata(file_name);
+	
+	x_lim				= [min([fstruct.x_lim(1) x_lim(1)]) max([fstruct.x_lim(2) x_lim(2)])];
+	y_lim				= [min([fstruct.y_lim(1) y_lim(1)]) max([fstruct.y_lim(2) y_lim(2)])];
+end
+
+grid_dx				= fstruct.grid_dx;
+grid_dy				= fstruct.grid_dy;
+Nx						= size(fstruct.source_image, 1);
+Ny						= size(fstruct.source_image, 2);
+
+fprintf('done\n');
+
+%% resample images onto common grid
+
+fprintf('Resampling images onto common grid ... ');
+
+x						= linspace(x_lim(1), x_lim(2), Nx);
+y						= linspace(y_lim(1), y_lim(2), Ny);
+dx						= x(2) - x(1);
+dy						= y(2) - y(1);
+[xmat, ymat]		= ndgrid(x, y);
+
+dewarped_image_mat= zeros(n_calibrations, Nx, Ny);
+for calib_idx = 1 : n_calibrations
+	file_idx			= calib_file_range(calib_idx);
+	file_name		= sprintf([calibration_dir '/' calib_infile_pat], file_idx);
+	fstruct			= importdata(file_name);
+	
+	source_image	= fstruct.source_image;
+	xytilde			= [xmat(:) ymat(:) ones(Nx*Ny,1)]';
+	ijtilde			= fstruct.P * xytilde;
+	imat				= reshape(ijtilde(1,:) ./ ijtilde(3,:), [Nx Ny]);
+	jmat				= reshape(ijtilde(2,:) ./ ijtilde(3,:), [Nx Ny]);
+	i_axis			= fstruct.i_axis;
+	j_axis			= fstruct.j_axis;
+	dewarped_image = interp2(i_axis, j_axis, source_image', imat, jmat, 'linear', nan);
+	
+	dewarped_image_mat(calib_idx, :, :) = dewarped_image;
+end
+
+fprintf('done\n');
+
+%% identify which markers are in which images
+
+x_lim_grid			= fix(x_lim/grid_dx) * grid_dx;
+y_lim_grid			= fix(y_lim/grid_dy) * grid_dy;
+x_grid				= x_lim_grid(1) : grid_dx : x_lim_grid(2);
+y_grid				= y_lim_grid(1) : grid_dy : y_lim_grid(2);
+Nx_grid				= length(x_grid);
+Ny_grid				= length(y_grid);
+
+[x_grid_mat, y_grid_mat] = ndgrid(x_grid, y_grid);
+n_shared_views		= zeros(Nx_grid, Ny_grid);
+b_marker_visible	= false(Nx_grid, Ny_grid, n_calibrations);
+
+fprintf('Searching for common markers ... ');
+
+for ix = 1 : Nx_grid
+	for iy = 1 : Ny_grid
+		%% find which images contain this marker
+		% extrapolated values in images are nans
+		% so markers we cannot see have nans near them
+		marker_is_visible	= false(1, n_calibrations);
+		xrange				= abs(x - x_grid(ix)) < marker_size(1);
+		yrange				= abs(y - y_grid(iy)) < marker_size(2);
+		for calib_idx = 1 : n_calibrations
+			dewarped_image		= dewarped_image_mat(calib_idx, :, :);
+			dewarped_image		= squeeze(dewarped_image);
+			marker_image		= dewarped_image(xrange,yrange);
+			marker_brightness = mean(marker_image(:));
+			marker_is_visible(calib_idx) = ...
+				all(~isnan(marker_image(:))) && marker_brightness > marker_min_bright;
+		end
+		n_shared_views(ix,iy)			= sum(marker_is_visible);
+		b_marker_visible(ix, iy, :)	= marker_is_visible;
+	end
+end
+
+fprintf('done\n');
+
+%% report on what markers are common 
+b_all_common			= n_shared_views == n_calibrations;
+b_some_common			= n_shared_views >= 2;
+n_all_common			= sum(b_all_common(:));
+n_some_common			= sum(b_some_common(:));
+
+fprintf('Found %d markers common across all views\n', n_all_common);
+fprintf('Found %d markers in at least 2 views\n', n_some_common);
+
+sum_of_all_images		= mean(dewarped_image_mat, 1);
+sum_of_all_images		= squeeze(sum_of_all_images);
+
+figure(1);
+hold off;
+imagesc(x, y, sum_of_all_images');
+hold on;
+plot(x_grid_mat(b_all_common ), y_grid_mat(b_all_common ), 'go');
+plot(x_grid_mat(b_some_common), y_grid_mat(b_some_common), 'rx');
+axis equal tight xy;
+colormap gray;
+xlim(x_lim);
+ylim(y_lim);
+title('Average of all images');
+xlabel('x / mm');
+ylabel('y / mm');
+
+sInput					= input('Do you want to check markers? (y): ', 's');
+if strcmp(sInput, 'y')
+	for calib_idx = 1 : n_calibrations
+		dewarped_image		= squeeze(dewarped_image_mat(calib_idx, :, :));
+		marker_visible		= squeeze(b_marker_visible(:,:,calib_idx));
+		
+		figure(1);
+		hold off;
+		imagesc(x, y, dewarped_image');
+		axis equal tight xy;
+		colormap gray;
+		xlim(x_lim);
+		ylim(y_lim);
+		hold on;
+		plot(x_grid_mat(marker_visible), y_grid_mat(marker_visible), 'go');
+		
+		fprintf('Image %d of %d\n', calib_idx, n_calibrations);
+		pause();
+	end
+end
+
+%% cross-correlate to find offset
+
+fprintf('Finding optimal shift of markers ... ');
+
+marker_shift_x			= zeros(Nx_grid, Ny_grid, n_calibrations);
+marker_shift_y			= zeros(Nx_grid, Ny_grid, n_calibrations);
+
+for ix = 1 : Nx_grid
+	for iy = 1 : Ny_grid
+		%% can't make a correction if there are <2 views
+		if n_shared_views(ix, iy) < 2
+			continue;
+		end
+		
+		%% get image of marker in first view
+		i_shared_views	= find(b_marker_visible(ix, iy, :));		
+		xrange			= abs(x - x_grid(ix)) < marker_size(1);
+		yrange			= abs(y - y_grid(iy)) < marker_size(2);
+		first_view_idx	= i_shared_views(1);
+		first_view_img	= dewarped_image_mat(first_view_idx, xrange, yrange);
+		first_view_img = squeeze(first_view_img);
+		first_view_img = first_view_img - mean(first_view_img(:));
+		first_view_e	= sum(first_view_img(:).^2);
+		
+		%% cross correlate to find offset
+		for iv = 2 : n_shared_views(ix, iy)
+			other_view_idx = i_shared_views(iv);
+			other_view_img = dewarped_image_mat(other_view_idx, xrange, yrange);
+			other_view_img = squeeze(other_view_img);
+			other_view_img = other_view_img - mean(other_view_img(:));
+			other_view_e	= sum(other_view_img(:).^2);
+			
+			xx					= xcorr2(first_view_img, other_view_img);
+			xx					= xx / sqrt(first_view_e * other_view_e);
+			peak_mag			= max(xx(:));
+			peak_loc			= PIV_2d_peaklocate(xx);
+			
+			% check for sufficient correlation
+			if peak_mag < peak_mag_threshold
+				fprintf('bad match marker (%0.1f,%0.2f) in view %d\n', x_grid(ix), y_grid(iy), other_view_idx);
+				marker_shift= [0 0];
+				b_marker_visible(ix, iy, iv) = false;
+			else
+				marker_shift= (peak_loc(:)' - size(first_view_img)) .* [dx dy];
+			end
+			
+			marker_shift_x(ix, iy, other_view_idx) = -marker_shift(1);
+			marker_shift_y(ix, iy, other_view_idx) = -marker_shift(2);
+		end
+		
+		% update which views share an image of this marker
+		i_shared_views		= find(b_marker_visible(ix, iy, :));		
+		
+		%% average out shifts so no image is preferred
+		average_shift_x	= mean(marker_shift_x(ix, iy, i_shared_views));
+		average_shift_y	= mean(marker_shift_y(ix, iy, i_shared_views));
+		marker_shift_x(ix, iy, i_shared_views) = marker_shift_x(ix, iy, i_shared_views) - average_shift_x;
+		marker_shift_y(ix, iy, i_shared_views) = marker_shift_y(ix, iy, i_shared_views) - average_shift_y;
+	end
+end
+
+fprintf('done\n');
+
+%% re-calibrate and re-dewarp
+
+fprintf('Recomputing calibration ... \n');
+
+rewarped_image_mat		= dewarped_image_mat;
+
+for calib_idx = 1 : n_calibrations
+	%% report
+	fprintf(' - calibration %2d of %2d ... ', calib_idx, n_calibrations);
+	
+	%% find markers on this image
+	b_visible_markers		= b_marker_visible(:, :, calib_idx);
+	marker_x					= x_grid_mat + marker_shift_x(:, :, calib_idx);
+	marker_y					= y_grid_mat + marker_shift_y(:, :, calib_idx);
+	marker_x_old			= marker_x(b_visible_markers);
+	marker_y_old			= marker_y(b_visible_markers);
+	marker_x_new			= x_grid_mat(b_visible_markers);
+	marker_y_new			= y_grid_mat(b_visible_markers);
+	n_visible_markers		= sum(b_visible_markers(:));
+	
+	%% get updated i/j coordinates of marker positions
+	file_idx					= calib_file_range(calib_idx);
+	file_name				= sprintf([calibration_dir '/' calib_infile_pat], file_idx);
+	fstruct					= importdata(file_name);
+	[marker_i,marker_j]	= pinholemap_2d(marker_x_old(:), marker_y_old(:), fstruct.P);
+	
+	%% recalibrate
+	marker_ij_tilde		= [marker_i(:) marker_j(:) ones(n_visible_markers,1)]';
+	marker_xy_tilde		= [marker_x_new(:) marker_y_new(:) ones(n_visible_markers,1)]';
+	P2D_new					= dlt33(marker_ij_tilde, marker_xy_tilde);
+	
+	%% re-dewarp image
+	im							= fstruct.source_image;
+	i_axis					= fstruct.i_axis;
+	j_axis					= fstruct.j_axis;
+	
+	[imat,jmat]				= pinholemap_2d(xmat, ymat, P2D_new);
+	rewarped_img			= interp2(i_axis, j_axis, im', imat, jmat, 'linear', nan);
+	rewarped_image_mat(calib_idx, :, :) = rewarped_img;
+	
+	%% save
+	fstruct.P					= P2D_new;
+	fstruct.ij_markers		= [marker_i(:) marker_j(:)]';
+	fstruct.xy_markers		= [marker_x_new(:) marker_y_new(:)]';
+	fstruct.x_axis				= x;
+	fstruct.y_axis				= y;
+	fstruct.dewarped_image	= rewarped_img;
+	
+	output_file					= sprintf([calibration_dir '/' calib_outfile_pat], file_idx);
+	save(output_file, '-struct', 'fstruct');
+	
+	fprintf('done\n');
+end
+
+fprintf('Done\n');
+
+%% compare dewarped and re-dewarped images
+
+for img_idx = 1 : n_calibrations
+	discrepancy_x	= squeeze( marker_shift_x(:,:, img_idx) );
+	discrepancy_y  = squeeze( marker_shift_y(:,:, img_idx) );
+	marker_x			= x_grid_mat + discrepancy_x;
+	marker_y			= y_grid_mat + discrepancy_y;
+	dewarped_img	= squeeze(dewarped_image_mat(img_idx, :, :));
+	rewarped_img	= squeeze(rewarped_image_mat(img_idx, :, :));
+
+	figure(2);
+	%{
+	subplot(1, 2, 1);
+	hold off;
+	imagesc(x, y, dewarped_img');
+	hold on;
+	plot(marker_x(:), marker_y(:), 'rx');
+	axis xy equal tight;
+	grid on;
+	xlim(x_lim);
+	ylim(y_lim);
+	%xlim([-25 25]);
+	%ylim([-25 25]);
+	title(sprintf('dewarped %d', img_idx)); 
+	
+
+	subplot(1, 2, 2);
+	%}
+	hold off;
+	imagesc(x, y, rewarped_img');
+	colormap jet;
+	grid on;
+	axis xy equal tight;
+	xlim(x_lim);
+	ylim(y_lim);
+	title(sprintf('rewarped %d', img_idx)); 
+	
+	pause();
+end

calibration/cubicfit_2d.m

+function C = cubicfit_2d(i, j, X, Niter)
+	% fits coefficients C to observations of X(i,j) to create a model of X
+	% according to
+	% X* =  C(1) + C(2)*i    + C(3)*i.^2    + C(4)*i.^3 + ...
+	%				 + C(5)*j    + C(6)*j.^2    + C(7)*j.^3 + ...
+	%				 + C(8)*i.*j + C(9)*i.^2.*j + C(10)*i.*j.^2;
+	%{
+	ii				= i.^2;
+	iii			= i.^3;
+	jj				= j.^2;
+	jjj			= j.^3;
+	ij				= i.*j;
+	iij			= i.^2.*i;
+	ijj			= i.*j.^2;
+	Np				= numel(i);
+	%}	
+	myfun			= @(X,xdata) cubicmap_2d(xdata(:,1), xdata(:,2), X);
+	C0				= zeros(1,10);
+	opts			= optimset('MaxFunEvals', 100000, 'MaxIter', 5000, 'Display', 'off');
+	[C,~,res]	= lsqcurvefit(myfun, C0, [i(:) j(:)], X(:), [], [], opts);
+	if nargin > 3 	
+		for iter = 2 : Niter
+			% cut out the chaff
+			thresh	= prctile(res, 97);
+			ok			= res < thresh;
+			i			= i(ok);
+			j			= j(ok);
+			X			= X(ok);
+			% repeat			
+			[C,~,res]= lsqcurvefit(myfun, C0, [i(:) j(:)], X(:), [], [], opts);
+		end
+	end
+	
+	
+	%{
+	A				= [ones(Np,1) i(:) ii(:) iii(:) j(:) jj(:) jjj(:) ij(:) iij(:) ijj(:)];
+	b				= X(:);	
+	[C,~,res]	= lsqlin(A, b, [], []);	
+	
+	% iteratively remove outliers
+	if nargin > 3 	
+		for iter = 2 : Niter
+			% cut out the chaff
+			thresh	= prctile(res, 97);
+			ok			= res < thresh;
+			i			= i(ok);
+			ii			= ii(ok);
+			iii		= iii(ok);
+			j			= j(ok);
+			jj			= jj(ok);
+			jjj		= jjj(ok);
+			ij			= ij(ok);
+			iij		= iij(ok);
+			ijj		= ijj(ok);
+			X			= X(ok);
+			Np			= numel(i);
+			% repeat
+			A			= [ones(Np,1) i(:) ii(:) iii(:) j(:) jj(:) jjj(:) ij(:) iij(:) ijj(:)];
+			b			= X(:);	
+			[C,~,res]= lsqlin(A, b, [], []);
+		end
+	end
+	%}
+end
\ No newline at end of file

calibration/cubicmap_2d.m

+function X = cubicmap_2d(i, j, C)
+	% maps [i,j] onto X = X(i,j) using coefficients provided in C
+	% coefficients derived from cubicfit_2d
+	X = C(1) + C(2)*i    + C(3)*i.^2    + C(4)*i.^3 + ...
+				+ C(5)*j    + C(6)*j.^2    + C(7)*j.^3 + ...
+				+ C(8)*i.*j + C(9)*i.^2.*j + C(10)*i.*j.^2;
+end
\ No newline at end of file

calibration/dewarp_2d.m

+function dewarped_img = dewarp_2d(raw_img, xy_to_i, xy_to_j, x, y)
+	% reinterpolates raw_img onto grid [x,y] from [i,j] using the dewarping coefficients
+	% xy_to_i and xy_to_j
+	
+	Nx					= length(x);
+	Ny					= length(y);
+	[xmat, ymat]	= ndgrid(x, y);
+	imat				= cubicmap_2d(xmat, ymat, xy_to_i);
+	jmat				= cubicmap_2d(xmat, ymat, xy_to_j);
+	
+	ivec				= 1 : size(raw_img, 1);
+	jvec				= 1 : size(raw_img, 2);	
+	dewarped_img	= interp2(ivec, jvec, double(raw_img'), imat, jmat, 'linear', 0);
+	
+	% lanczos interpolation is faster than matlab's interp2 and better
+	% quality. kernel size is specified by ksize. 18 is good.
+	%{
+	ksize				= 10;
+	xi					= [imat(:)-1, jmat(:)-1]';
+	dewarped_img	= interp2lanczos(single(raw_img), single(xi), ksize);
+	dewarped_img	= reshape(dewarped_img, Nx, Ny);
+	%}
+end
\ No newline at end of file

calibration/dewarp_pinhole_2d.m

+function im_uv = dewarp_pinhole_2d(im, P2D, u_axis, v_axis)
+	Nu						= length(u_axis);
+	Nv						= length(v_axis);
+	[umat, vmat]		= ndgrid(u_axis, v_axis);
+	uv_tilde				= [umat(:) vmat(:) ones(Nu*Nv,1)]';
+	ij_tilde				= P2D * uv_tilde;
+	lambda				= ij_tilde(3, :);
+	imat					= reshape(ij_tilde(1,:) ./ lambda, [Nu Nv]);
+	jmat					= reshape(ij_tilde(2,:) ./ lambda, [Nu Nv]);
+	i_axis				= 1 : size(im, 1);
+	j_axis				= 1 : size(im, 2);
+	im_uv					= interp2(i_axis, j_axis, im', imat, jmat, 'linear');
+end
\ No newline at end of file

calibration/distortion_test.m

+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% fit calibration data obtained from pinhole camera model fit
+% to a higher order camera model
+%
+% we follow Machioane et al. 2016 to extract a transfer function that
+% maps image space coordinates to rays through object space
+%
+% 1) obtain 2D transfer function for each calibration plane
+% 2) trace rays back for each pixel, based on intersection
+%    with each calibration plane
+% 3) fit a transfer function to map rays to 
+
+addpath([pwd '/pinhole']);
+addpath([pwd '/poly']);
+
+%% input parameters
+
+calib_dir					= '/nfs/data/lawson/160715/calib/';
+cam_calib_file_pat		= [calib_dir '/CAM%d-%02d.mat'];
+cam_model_file				= [calib_dir '/camera-model-original.mat'];
+
+cam_index_range			= [1 2 3];
+cam_view_range				= 1 : 16;
+n_views						= length(cam_view_range);
+n_cameras					= length(cam_index_range);
+n_iterations				= 2;
+
+DO_DEBUG_PLOT				= false;
+
+%% 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);
+n_views						= setup.n_views;
+
+%% obtain 2D transfer function for each plane
+fprintf('Fitting 2D transfer function for each plate in each camera\n');
+fprintf('This gives best case scenario for how advanced model will reduce error\n');
+
+quiver_scale				= 10;
+for c = 1 : n_cameras
+	%% initialise
+	camera					= setup.camera(c);
+	i_axis					= camera.i_axis;
+	j_axis					= camera.j_axis;
+	ij_res_poly				= [];
+	ij_res_pin				= [];
+	
+	gl_mark_ary				= [];
+	ij_mark_ary				= [];
+	sh_mark_ary				= [];
+	
+	%% loop over views
+	for v = 1 : n_views
+		%% get plate, image and world coordinates of markers
+		pl_to_gl					= pl_to_gl_mat(:,:,v);
+		gl_pos					= gl_plate_pos(:,v);
+		uv_mark					= camera.view(v).uv_markers;
+		n_mark					= size(uv_mark, 2);
+		gl_mark					= bsxfun(@plus, gl_pos, pl_to_gl*[uv_mark; zeros(1,n_mark)]);
+		ij_mark					= camera.view(v).ij_markers;
+		
+		%% get sphere coordinates of markers
+		R							= camera.calib.R;
+		xc							= camera.calib.xc;
+		eu_mark					= R * bsxfun(@minus, gl_mark, xc);
+		eu_len					= sqrt(sum(eu_mark.^2,1));
+		eu_mark					= bsxfun(@rdivide, eu_mark, eu_len);
+		theta_mark				= acos(eu_mark(3,:));
+		phi_mark					= atan2(eu_mark(2,:), eu_mark(1,:));
+		
+		sh_mark					= [theta_mark; phi_mark];
+		
+		ij_mark_ary				= [ij_mark_ary, ij_mark];
+		gl_mark_ary				= [gl_mark_ary, gl_mark];
+		sh_mark_ary				= [sh_mark_ary, sh_mark];
+		
+		%% create transfer function
+		P_i2p						= polyfit2d3(ij_mark, uv_mark);
+		P_p2i						= polyfit2d3(uv_mark, ij_mark);
+		camera.view(v).P_i2p	= P_i2p;
+		camera.view(v).P_p2i	= P_p2i;
+		
+		%% compare transfer function to pinhole model
+		uv_res					= polymap2d3(P_i2p, ij_mark) - uv_mark;
+		ij_res_poly_view		= polymap2d3(P_p2i, uv_mark) - ij_mark;
+		ij_res_pin_view		= pinhole_model(gl_mark, camera.calib, true) - ij_mark;
+		
+		ij_res_poly				= [ij_res_poly, ij_res_poly_view];
+		ij_res_pin				= [ij_res_pin, ij_res_pin_view];
+		
+		%% debug plot
+		if DO_DEBUG_PLOT
+			%% residuals
+			figure(1);
+			clf;
+			quiversc(ij_mark(1,:), ij_mark(2,:), ...
+						ij_res_poly_view(1,:), ij_res_poly_view(2,:), quiver_scale, 'k-');
+			hold on;
+			quiversc(ij_mark(1,:), ij_mark(2,:), ...
+						ij_res_pin_view(1,:), ij_res_pin_view(2,:), quiver_scale, 'r-');
+			axis equal xy;
+			xlim(i_axis([1 end]));
+			ylim(j_axis([1 end]));
+			%%
+			pause();
+		end
+	end
+	
+	%% polynomial map, undistorted image -> image
+	ij_proj_ary		= pinhole_model(gl_mark_ary, camera.calib, false);
+	ij_res_ary		= ij_proj_ary - ij_mark_ary;
+	P_u2d				= polyfit2d4(ij_proj_ary, ij_mark_ary);
+	P_d2u				= polyfit2d4(ij_mark_ary, ij_proj_ary);
+	
+	ij_dist_ary		= polymap2d4(P_u2d, ij_proj_ary);
+	ij_res_pp		= ij_dist_ary - ij_mark_ary;
+	
+	%% polynomial map, spherical -> image
+	P_s2i				= polyfit2d3(sh_mark_ary, ij_mark_ary);
+	P_i2s				= polyfit2d3(ij_mark_ary, sh_mark_ary);
+	camera.calib.P_s2i	= P_s2i;
+	camera.calib.P_i2s	= P_i2s;
+	
+	%% mapping
+	figure(2);
+	quiversc(ij_mark_ary(1,:), ij_mark_ary(2,:), ...
+				ij_res_pp(1,:), ij_res_pp(2,:), 2, 'k-');
+	axis equal tight xy;
+	xlim(i_axis([1 end]));
+	ylim(j_axis([1 end]));
+	
+	%% save
+	setup.camera(c)	= camera;
+	
+	%% report differences
+	ij_rms_poly	= rms(ij_res_poly, 2);
+	ij_rms_pin	= rms(ij_res_pin, 2);
+	ij_rms_pp	= rms(ij_res_pp, 2);
+	ij_max_poly	= max(abs(ij_res_poly), [], 2);
+	ij_max_pin  = max(abs(ij_res_pin ), [], 2);
+	ij_max_pp	= max(abs(ij_res_pp  ), [], 2);
+	fprintf('Residuals in camera %d\n', c);
+	fprintf('   polynomial:   %0.3f %0.3f px rms %0.3f %0.3f max\n', ij_rms_poly, ij_max_poly);
+	fprintf('   pinhole+dist: %0.3f %0.3f px rms %0.3f %0.3f max\n', ij_rms_pp  , ij_max_pp);
+	fprintf('   pinhole+rad:  %0.3f %0.3f px rms %0.3f %0.3f max\n', ij_rms_pin , ij_max_pin);
+end
+

calibration/dlt/dlt_maths.m

+%% work out maths for parallel DLT
+% we have a problem where
+% y1_k ~ T1 * x_k
+% y2_k ~ T2 * x_k
+% but we don't know x_k, or T1 or T2
+%
+% so this can be written as
+% a_k * y1_k = T1 * x_k
+% b_k * y2_k = T2 * x_k
+% for k = 1 .. N equations
+%
+% if we choose an antisymmetric matrix Hi, then
+% a_k * y1_k' * Hi * y2_k = y1_k' * Hi * T1 * x_k
+%                         = 0
+% b_k * y2_k' * Hj * y2_k = y2_k' * Hj * T2 * x_k
+%                         = 0
+
+%% set up T1 and T2
+
+% we need two random orthogonal matrices
+Omega1		= randortho(3);
+Omega2		= randortho(3);
+% and random displacements xc1 and xc2
+xc1			= randn(3,1) * 100 + [0 1000 0]';
+xc2			= randn(3,1) * 100 + [1000 0 0]';
+T1				= Omega1 * [eye(3) -xc1];
+T2				= Omega2 * [eye(3) -xc2];
+T				= [T1; T2];
+
+%% set up observations of xk, y1k and y2k
+
+N				= 1000;
+xk				= randn(3,N) * 100;
+xk_tilde		= [xk; ones(1,N)];
+y1k			= T1 * xk;
+y2k			= T2 * xk;
+zk				= [y1k; y2k];
+
+%% generate initial guess of xk, Omega1/2, xc1 and xc2
+
+xk_guess		= xk;
+SNR			= 100;
+for k = 1 : N
+	xk_guess(:,k) = xk(:,k) + randn(3,1)*norm(xk(:,k))/SNR;
+end
+
+omega1		= vrrotmat2vec(Omega1);
+omega2		= vrrotmat2vec(Omega2);
+omega1		= omega1(1:3) * omega1(4);
+omega2		= omega2(1:3) * omega2(4);
+omega1_guess= omega1 + randn(3,1) * norm(omega1) / SNR;
+omega2_guess= omega2 + randn(3,1) * norm(omega2) / SNR;
+xc1_guess	= xc1 + norm(xc1)*randn(3,1)/SNR;
+xc2_guess	= xc2 + norm(xc2)*randn(3,1)/SNR;
+
+
+
+%% generate initial guess for model
+

calibration/dlt/dlt_model.m

+function residuals = dlt_model(params, y1k, y2k, xc1, xc2)
+	%% extract model parameters
+	omega1	= params(1:3);
+	omega2	= params(4:6);
+	delta		= [ 0 0 0]';%params(7:9);
+	xc1_prime= xc1(:) + delta(:);
+	xc2_prime= xc2(:) + delta(:);
+	
+	% get rotation matrices
+	theta1	= norm(omega1);
+	theta2	= norm(omega2);
+	omega1	= [omega1/theta1; theta1];
+	omega2	= [omega2/theta2; theta2];
+	if theta1 < 1E-6; omega1 = [1 0 0 0]; end
+	if theta2 < 1E-6; omega2 = [1 0 0 0]; end
+	Omega1	= vrrotvec2mat(omega1);
+	Omega2	= vrrotvec2mat(omega2);
+	
+	% get T1 and T2
+	T1			= Omega1 * [eye(3), -xc1_prime];
+	T2			= Omega2 * [eye(3), -xc2_prime];
+	
+	% get model of xk
+	N			= (length(params) - 6) / 3;
+	xk			= reshape(params(7:end), [3 N]);
+
+	%% produce model result
+	H1			= [0 0 0; 0 0 -1; 0 1 0];
+	H2			= [0 0 1; 0 0 0; -1 0 0];
+	H3			= [0 -1 0; 1 0 0; 0 0 0];
+	residuals= zeros(6*N, 1);
+	for k = 1 : N
+		residuals((k-1)*6 + 1) = y1k(:,k)' * H1 * T1 * [xk(:,k); 1];
+		residuals((k-1)*6 + 2) = y1k(:,k)' * H2 * T1 * [xk(:,k); 1];
+		residuals((k-1)*6 + 3) = y1k(:,k)' * H3 * T1 * [xk(:,k); 1];
+		residuals((k-1)*6 + 4) = y2k(:,k)' * H1 * T2 * [xk(:,k); 1];
+		residuals((k-1)*6 + 5) = y2k(:,k)' * H2 * T2 * [xk(:,k); 1];
+		residuals((k-1)*6 + 6) = y2k(:,k)' * H3 * T2 * [xk(:,k); 1];
+	end
+end
\ No newline at end of file