% Load scans from file

load corridor1_matlab; scans = corridor1; 

sigma = 0.005;
scan1 = scans{1};
scan2 = scans{4};


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

% 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);

%%%%%% HSM starts here

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

fprintf('Maximum norm: %f\n', p.max_norm);

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

fprintf('Finding normals for scan1...\n')
scan1 =  h3d_scan_find_normals(h3d_scan_convert_points(scan1));
fprintf('Finding normals for scan2...\n')
scan2 =  h3d_scan_find_normals(h3d_scan_convert_points(scan2));

base = h3d_create_struct(p.angular_cell_size_deg, p.rho_cell_size, -p.max_norm, p.max_norm);

fprintf('Computing HT for scan1...\n')
htn1 =  h3d_compute_cube_ht_normals(p, base, scan1, eye(3), [0;0;0]);
fprintf('Computing HT for scan2...\n')
htn2 =  h3d_compute_cube_ht_normals(p, base, scan2, eye(3), [0;0;0]);


h3d_debug_display_hs(htn1);
h3d_debug_display_hs(htn2);

	if true
		figure;
		subplot(2,1,1);
		h3d_draw_sphere_hs(htn1, 100);
		subplot(2,1,2);
		h3d_draw_sphere_hs(htn2, 100);
		
		h3d_display_clouds(scan1.points,true_r,true_t,scan2.points,eye(3),[0;0;0],1)

	end



	[Rs, values] = h3d_guess_rotation(p, htn1, htn2);

	% list of {rot, trans}
	results = {};
	for i=1:numel(Rs)
		[Ts, values] = h3d_guess_translation(p, htn1, htn2, Rs{i});
		for k=1:numel(Ts)
			j = numel(results) + 1;
			results{j} = {Rs{i}, Ts{k}};
		end
	end

	test_points = 200;
	plausible_threshold = 0.3;
	
	[results, results_values, results_stats] = h3d_evaluate_solutions_hausdorff(p, scan1, scan2,eye(3),[0;0;0], results,p.hausdorff_num_points,p.hausdorff_plausible_threshold);


	h3d_debug_write_summary(p, results, results_values, results_stats);
	
	if true
		h3d_debug_display_some(scan1, scan2,results);
	end
	
%	h3d_display_clouds(scan1.points,p.debug_true_R,p.debug_true_T,scan2.points,eye(3),[0;0;0],1)
%	b=3; h3d_display_clouds(scan1.points,results{b}{1},results{b}{2},scan2.points,eye(3),[0;0;0],1)