%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% test pinhole camera fit routine

%% pinhole model to test
G			= [7715.9132      8.9361442     -443.64006
				  0     -7774.5787       468.7479
				  0              0              1];
r_vec		= [-0.6550    0.0087    0.7555    3.1363];
R			= vrrotvec2mat(r_vec);
xc			= [1098.6917     -14.767852     -20.153438]';

kr			= [5E-8 -2E-15];
ic			= [512 512];

P			= G*R*[eye(3), -xc(:)];

calib		= struct('P', P, 'G', G, 'R', R, 'xc', xc, 'kr', kr, 'ic', ic);

%% project points in object space
x_vec		= -10 : 10 : 10;
y_vec		= -60 : 10 : 60;
z_vec		= -60 : 10 : 60;

[xmat,ymat,zmat] = ndgrid(x_vec, y_vec, z_vec);
gl_mark	= [xmat(:) ymat(:) zmat(:)]';
ij_mark  = pinhole_model(gl_mark, calib);

%% apply fit
fprintf('fitting ... ');
[calib_fit, ij_res] = pinhole_fit_3d(ij_mark, gl_mark);
fprintf('done\n');
%{

calib_fit=calib;
calib_fit.kr = [0 0];
%}
ij_fit	= pinhole_model(gl_mark, calib_fit);
ij_res	= ij_mark - ij_fit;

%% plot
figure(1);
clf;
subplot(1,2,1);
cla;
hold on;
plot(	ij_mark(1,:), ij_mark(2,:), 'k.', ...
		ij_fit(1,:), ij_fit(2,:), 'kx');
quiver(ij_mark(1,:), ij_mark(2,:), ...
		 ij_res(1,:), ij_res(2,:), 'k-');
axis equal tight xy;

subplot(1,2,2);
cla;
plot3(gl_mark(1,:), gl_mark(2,:), gl_mark(3,:), 'r.');
axis equal tight xy;
view(3);
