function [new_model, ij_res] = pinhole_fit_3d(ij, uvw)
	% fit a set of points u,v,w to a pinhole model with radial distortion
	% a standard pinhole model maps u,v,w to distorted image plane
	% coordinates
	% i_d        u
	% j_d  ~ P * v
	%  1         w
	%
	% then distorted image plane coordinates are mapped to undistorted
	% coordinates using
	%
	% i_u = i_p + (i_d - i_p) * (1 + k1*r^2 + k2*r^4)
	% j_u = j_p + (j_d - j_p) * (1 + k1*r^2 + k2*r^4)
	% r   = (i_d - i_p)^2 + (j_d - j_p)^2
	% where i_p and j_p are the coordinates of the principal point, which is
	% determined by P, and k1 and k2 are radial distortion parameters
	%
	% in the first step, we solve for P using a direct linear transform
	% as an initial guess. in the second, we apply a least squares model 
	% to solve for P and the radial distortion parameters
	% which minimises disparity between model and data
	% 
	
	%% preliminary fit
	% solve using direct linear transform
	N			= size(uvw, 2); 
	assert(size(ij ,2) == N);
	assert(size(uvw,1) == 3);
	assert(size(ij ,1) == 2);
	
	uvw1					= [uvw; ones(1,N)];
	P						= dlt34([ij; ones(1,N)], uvw1);
	
	%% decompose pinhole model
	% P ~ G*R*[eye(3) -xc]
	% xc is in the null space of p
	% G is upper triangular
	% [alpha_x gamma   -f*alpha_x
	%          alpha_y -f*alpha_y
	%                  1         ]
	% R is a rotation matrix
	xc						= null(P);
	xc						= xc(1:3) / xc(4);
	xc						= xc(:);
	% RQ decomposition to find upper triangular G and orthogonal R
	GR						= P(1:3, 1:3);
	[G,R]					= rq(GR);
	G						= G / G(3,3);
	% ensure R is right handed
	perm					= [sign(det(R)) 0 0; 0 1 0; 0 0 1];
	G						= G*perm;
	R						= perm*R;
	P						= G*R*[eye(3), -xc(:)];
	
	%% estimate radial distortion
	% we fit a radial distortion model to the residuals of the pinhole model
	% fit
	lambda				=  P(3,:) * uvw1;
	ij_model				= [P(1,:) * uvw1 ./ lambda;
								P(2,:) * uvw1 ./ lambda];
	X0						= [mean(ij(1,:)) mean(ij(2,:)) 0 0];
	
	opts					= optimset('Display', 'off', 'TolX', 1E-9);
	[X,n,res,flag]		= lsqcurvefit(@radial_distortion_model, X0, ij_model, ij, [], [], opts);
	ic0					= X(1);
	jc0					= X(2);
	kr0					= X(3:4);
	
	%% incorporate into preliminary model
	% get approximate diagonal of sensor
	% used in normalisation stage
	diag_px				= norm(max(ij,[],2) - min(ij,[],2));
	model					= struct('P', P, 'G', G, 'R', R, 'xc', xc, 'kr', kr0, 'ic', [ic0 jc0], 'diag', diag_px);		

	%% 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
	X0						= pack_model(model);
	opts					= optimset('Display', 'off', 'TolX', 1E-9);
	[X, ~, ij_res]		= lsqcurvefit(@(X,uvw)fitfun(X,uvw,diag_px), X0, uvw, ij, [], [], opts);
	new_model			= unpack_model(X, diag_px);
end

function ij_dist = radial_distortion_model(X, ij_undist)
	% X(1) = ic
	% X(2) = jc
	% X(3) = k1
	% X(4) = k2
	iprime	= ij_undist(1,:) - X(1);
	jprime	= ij_undist(2,:) - X(2);
	rsq		= iprime.^2 + jprime.^2;
	ij_dist	= [X(1) + iprime .* (1 + X(3)*rsq + X(4)*rsq.^2);
					X(2) + jprime .* (1 + X(3)*rsq + X(4)*rsq.^2)];
end

function X = pack_model(model)
	X			= zeros(1,15);
	% intrinsic parameters
	X(1)		= model.G(1,1);
	X(2)		= model.G(1,2);
	X(3)		= model.G(1,3) / model.diag;
	X(4)		= model.G(2,2);
	X(5)		= model.G(2,3) / model.diag;
	
	X(6)		= model.ic(1) / model.diag;
	X(7)		= model.ic(2) / model.diag;
	X(8)		= model.kr(1) * model.diag^2;
	X(9)		= model.kr(2) * model.diag^4;
	
	% extrinsic parameters
	r_vec		= vrrotmat2vec(model.R);
	X(10:12) = r_vec(1:3) * r_vec(4);
	X(13:15) = model.xc(:)';
end

function model = unpack_model(X, diag_px)
	% intrinsic parameters
	fx			= X(1);
	tau		= X(2);
	ox			= X(3) * diag_px;
	fy			= X(4);
	oy			= X(5) * diag_px;

	ic			= X(6:7) * diag_px;
	kr			= [X(8)/diag_px^2 X(9)/diag_px^4];
	
	% extrinsic parameters
	r_vec		= X(10:12);
	R			= vrrotvec2mat([r_vec(:)/(eps+norm(r_vec)); norm(r_vec)]);
	xc			= X(13:15)';
	
	% put in structure
	G			= [fx tau ox;
					0   fy oy;
					0   0  1];
	P			= G*R*[eye(3), -xc(:)];
	
	model		= struct('P', P, 'G', G, 'R', R, 'xc', xc, ...
							'kr', kr, 'ic', ic, 'diag', diag_px);		
end

function ij = fitfun(X, uvw, diag_px)
	model		= unpack_model(X, diag_px);
	ij			= pinhole_model(uvw, model);
end

