function [calib, imageResiduals] = pinhole_fit_geometric(imagePoints, worldPoints, constraints)
	% use the Gold Standard algorithm  (7.1) described in Hartley & Zisserman
	% to obtain a constrained estimate of the projection matrix P
	% from a set of image data
	
	%% number of similarity relations we have
	N			= size(imagePoints, 2);
	assert(size(imagePoints,2) == N);
	assert(size(imagePoints,1) == 2);
	assert(size(worldPoints,1) == 3);
	
	%% obtain first guess calibration by minimising algebraic error
	fg					= pinhole_fit_algebraic(imagePoints, worldPoints, constraints);
	
	%% least squares fit
	% we wish to incorporate radial distortion into the pinhole model
	% this is non-linear and requires a least squares fit
	% start fit
	StartPoint			= pack_model(fg);
	opts				= optimset('Display', 'on');
	
	LB					= -inf(15,1);
	UB					= +inf(15,1);
	if constraints.b_square
		LB(2)			= sign(fg.G(2,2)/fg.G(1,1));
		UB(2)			= LB(2);
	end
	if ~constraints.b_estimate_skew
		% turn off estimation of skew by setting it to zero
		LB(3)			= 0;
		UB(3)			= 0;
		StartPoint(3)	= 0;
	end
	if ~constraints.b_radial_distortion
		% turn off radial distortion by fixing bounds on coefficients
		StartPoint(8:9) = 0;
		LB(8:9)			= 0;
		UB(8:9)			= 0;
		LB(6:7)			= StartPoint(6:7);
		UB(6:7)			= StartPoint(6:7);
	end
	
	goalfun				= @(cmodel) residuals(cmodel, worldPoints, imagePoints);
	opts				= optimoptions('lsqnonlin', ...
							'Algorithm', 'levenberg-marquardt', ...
							'MaxIterations', 1E6, ...
							'MaxFunctionEvaluations', 1E6);
	[cmodel,~,~,flag]	= lsqnonlin(goalfun, StartPoint, LB, UB, opts);
	if flag ~= 1
		warning('least squares fitting of geometric error did not converge');
	end

	%% solve residuals with new model
	calib				= unpack_model(cmodel);
	imageResiduals		= pinhole_model(worldPoints, calib, true) - imagePoints;
end

function X = pack_model(model)
	X			= zeros(15,1);
	% intrinsic parameters
	G			= model.G;
	X(1)		= G(2,2);		% focal length
	X(2)		= G(1,1)/G(2,2);% aspect ratio
	X(3)		= G(1,2);		% skewness
	X(4)		= G(1,3);		% principal point x
	X(5)		= G(2,3);		% principal point y
	X(6)		= model.ic(1);	% x center of distortion
	X(7)		= model.ic(2);	% y center of distortion
	X(8)		= model.kr(1);
	X(9)		= model.kr(2);
	
	% extrinsic parameters
	r_vec		= vrrotmat2vec(model.R);
	X(10:12)	= r_vec(1:3) * r_vec(4);
	X(13:15)	= -model.R*model.xc; %translation t %model.xc(:)';
end

function model = unpack_model(X)
	% intrinsic parameters
	f			= X(1);			% focal length
	AR			= X(2);			% aspect ratio
	tau			= X(3);			% skewnewss
	ox			= X(4);			% x principal point
	oy			= X(5);			% y principal point
	ic			= [X(6) X(7)];	% center of distortion
	kr			= [X(8) X(9)];	% radial distortion coefficient
	
	% extrinsic parameters
	r_vec		= X(10:12);
	R			= vrrotvec2mat([r_vec(:)/(eps+norm(r_vec)); norm(r_vec)]);
	t			= X(13:15);
	t			= t(:);
	xc			= -R\t;
	
	% put in structure
	G			= [AR*f tau ox;
					0   f   oy;
					0   0   1];
	P			= G*[R t];
	
	model		= struct('P', P, 'G', G, 'R', R, 'xc', xc, ...
							'kr', kr, 'ic', ic);		
end

function res = residuals(cmodel, worldPoints, imagePoints)
	model		= unpack_model(cmodel);
	imageProj	= pinhole_model(worldPoints, model, true);
	res			= imagePoints - imageProj;
	res			= res(:);
end