p = h3d_params;
p.angular_cell_size_deg = 6;
p.guess_tran_two_birds_one_stone = 0;
p.guess_tran_max_directions = 4;

% Read logs from file

scans = flatrun;

scan1 = scans{3};
scan2 = scans{5};

sigma = 0.01;
scan1 = h3d_scan_add_noise(scan1, sigma);
scan2 = h3d_scan_add_noise(scan2, sigma);

% Compute cloud of points
sa = h3d_scan_convert_points(scan1);
sb = h3d_scan_convert_points(scan2);

% and sample intelligently some points
ndes = 3000;
fprintf('Sampling points from scan1...\n');
s1 = h3d_scan_sample_points(sa, ndes);
fprintf('Sampling points from scan2...\n');
s2 = h3d_scan_sample_points(sb, ndes);

points1 = s1.sampled_points;
points2 = s2.sampled_points;

% orientation
OR1 = rot_axis_angle(scan1.true_pose{1}{1},scan1.true_pose{1}{2})
OR2 = rot_axis_angle(scan2.true_pose{1}{1},scan2.true_pose{1}{2})
% pose
T1 = scan1.true_pose{2};
T2 = scan2.true_pose{2};

true_r = OR2' * OR1;
true_t =  OR2' *(T1 - T2);

h3d_display_clouds(s2.points,eye(3),[0;0;0],s1.points,true_r,true_t,1)


points1 = s1.sampled_points;
points2 = s2.sampled_points;


% 
if false
	delta = rot_axis_angle([1;1;1],2);
	points1 = delta*s1.sampled_points;
	points2 = delta*s2.sampled_points;


	% Let this be the true displacement

	true_t = [0.2;-0.2;0.2];
	%true_t = [0;0;0];
	true_r = rot_axis_angle([1;1;0], deg2rad(34));
	%true_r = rot_axis_angle([1;1;0], deg2rad(0));

	for i=1:size(points2,2)
		points2(:,i) = true_r * points2(:,i) + true_t;
	end
end



p.debug_true_R = true_r;
p.debug_true_T = true_t;

% Setup finished. Now we have p, points1 and points2.


max1 = max(max(scan1.readings));
max2 = max(max(scan2.readings));
p.max_norm = max([max1 max2]) *1.1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%





% Parameters for the HT
if true
	base = h3d_create_struct(p.angular_cell_size_deg, p.rho_cell_size, -p.max_norm, p.max_norm);
	ht1 = h3d_compute_cube_ht(p, base, points1);
	ht2 = h3d_compute_cube_ht(p, base, points2);
end

if false
	figure;
	subplot(2,1,1);
	h3d_draw_sphere_hs(ht1, 100);
	subplot(2,1,2);
	h3d_draw_sphere_hs(ht2, 100);
	
end


[Rs, values] = h3d_guess_rotation(p, ht1, ht2);

% list of {rot, trans}
results = {};
results_values = [];

for i=1:numel(Rs)
	[Ts, values] = h3d_guess_translation(p, ht1, ht2, Rs{i});
	for k=1:numel(Ts)
		j = numel(results) + 1;
		results{j} = {Rs{i}, Ts{k}};
		results_values(j) = values(k);
	end
end


% sort the hypotheses
[Y,I] = sort(results_values, 'descend');
results = {results{I}};
results_values = results_values(I);


fprintf('Summary:\n');
for i=1:min([numel(results) 30])

	R = results{i}{1};
	T = results{i}{2};
	val = results_values(i);
	
	if numel(p.debug_true_T) == 3
		e_r = h3d_rotation_error(R, p.debug_true_R);
		e_t = (T -  p.debug_true_T);
		s = sprintf('error R: %03.1f deg; T: %3.1f cm', e_r, norm(e_t) * 100);
	else
		s= '';
	end
	fprintf('%d Val = %1.3f   R = %s  T = %s   %s\n', i, val, display_rot_aa(R), display_v3(T),s);	
end

if numel(p.debug_true_T) == 3
	fprintf(' TRUE: R = %s  T = %s\n', display_rot_aa(p.debug_true_R), display_v3(p.debug_true_T));
end



if true
	figure; hold on;
	
	b = 2; h3d_display_clouds(points1,results{b}{1},results{b}{2},points2,eye(3),[0;0;0])

	%	s=plot3points(points1,'b.');
	%	set(s,'MarkerSize',0.5);
%	s=plot3points(R'*(points2- repmat(T,1,size(points2,2))),'r.');
%	set(s,'MarkerSize',0.5);
%	axis('equal');
	
	
end