%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% test computation of Jacobian in pinhole model

%% input parameters
calib_dir			= '/nfs/data.lfpn/lawson/160412/calib/';
calib_file			= 'camera-model-160418-0000-0099.mat';
n_pos					= 1000;
cam_idx				= 3;

dx						= rand(3,1);
dx						= dx / norm(dx);
dx						= 0.2 * dx;

%% load calibration
fname					= [calib_dir '/' calib_file];
setup					= importdata(fname);
camera				= setup.camera;
n_cameras			= setup.n_cameras;
r_sphere				= setup.r_sphere_lim;
gl_origin			= setup.gl_origin;

calib					= camera(cam_idx).calib;

%% generate random points in sphere
gl_ctr				= r_sphere * (0.5 - rand(3,n_pos));
gl_shift				= bsxfun(@plus, gl_ctr, dx);
gl_dx					= gl_shift - gl_ctr;

[ij_ctr, jac]		= pinhole_model(gl_ctr, calib, true);
ij_shift				= pinhole_model(gl_shift, calib, true);
ij_est				= ij_ctr + squeeze(mtimesx(jac, reshape(gl_dx, [3 1 n_pos])));

ij_res				= ij_est - ij_shift;
ij_res_rms			= rms(ij_res,2);
ij_mov				= ij_shift - ij_ctr;
ij_mov_rms			= rms(ij_mov,2);

fprintf('RMS displacment: %0.3f %0.3f px \n', ij_mov_rms);
fprintf('Error of estimator %0.3f %0.3f px\n', ij_res_rms);

%% plot
figure(1);
clf;
plot(ij_shift(1,:), ij_shift(2,:), 'k.');
hold on
plot(ij_ctr(1,:), ij_ctr(2,:), 'r.');
plot(ij_est(1,:), ij_est(2,:), 'ko');