function [new_setup, ij_res] = traverse_error_fit(setup)
	% try and model the error in traverse position
	% and calibration plate orientation
	%
	% we are given the setup of the calibration plate and traverse
	% and a model of the camera calibration
	%
	% we seek a small perturbation to the calibration plate's position and
	% orientation, that helps explain the discrepancy between the pinhole
	% camera model and observation
	
	%% extract basic information
	n_cameras	= setup.n_cameras;
	n_views		= setup.n_views;
	
	%% initialise perturbation model
	setup.gl_plate_omega = zeros(3, n_views);
	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
	X0				= pack_model(setup);
	NX				= length(X0);
	opts			= optimset('MaxFunEvals', NX*100, 'MaxIter', 400, 'Display', 'on');
	X				= lsqcurvefit(@(X,xdata) bundle_model(X,xdata,setup),...
									  X0, uv_data, ij_data, [], [], opts);
	new_setup	= unpack_model(X, setup);
end

function ij_mark = 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;
	n_views				= setup.n_views;
	gl_plate_omega		= setup.gl_plate_omega;
	pl_to_gl_mat		= zeros(3,3,n_views);
	for v = 1 : n_views
		omega_vec		= gl_plate_omega(:,v);
		Omega				= vrrotvec2mat([omega_vec(:)/(norm(omega_vec)+eps); norm(omega_vec)]);
		pl_to_gl_mat(:,:,v) = Omega * setup.pl_to_gl_mat(:,:,v);
	end
	
	%% project markers
	n_cameras			= setup.n_cameras;
	n_markers			= size(uv_mark, 1);
	cam					= uv_mark(:, 3);
	view					= uv_mark(:, 4);
	pl_mark				= [uv_mark(:, 1:2), zeros(n_markers,1)]'; 
	pl_mark				= reshape(pl_mark, [3 1 n_markers]);
	gl_mark				= squeeze(mtimesx(pl_to_gl_mat(:,:,view), pl_mark)) ...
							+ gl_plate_pos(:, view);
	% gl_mark should be 3xn_markers matrix
	
	% project markers
	ij_mark				= zeros(n_markers, 2);
	for c = 1 : n_cameras
		this_cam					= cam == c;
		ij_mark(this_cam,:)	= pinhole_model(gl_mark(:, this_cam), setup.camera(c).calib)';
	end
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
	
	%% plate perturbation model
	% plate perturbation consists of a displacement and rotation
	X_plate		= setup.gl_plate_dx([1 2 3], :);
%	omega_vec	= vrrotmat2vec(setup.omega);
	X_rot			= setup.gl_plate_omega;
	
	X				= [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;
	
	%% extract
	off			= 1;
	X_plate		= reshape(X(off : off-1 + n_views*3), [3 n_views]);
	off			= off + n_views*3;
	X_rot			= reshape(X(off : off-1 + n_views*3), [3 n_views]);
	
	%% update plate position parameters
	%omega_vec	= [X_rot(:)/(eps+norm(X_rot)); norm(X_rot)];
	%setup.omega = vrrotvec2mat(omega_vec);
	setup.gl_plate_omega = X_rot;
	setup.gl_plate_dx		= X_plate;%[X_plate(1,:); zeros(1,n_views); X_plate(2,:)]; 
end
