%load scan1.mat

% Load two sets of points

tot = size(scan1,2);
every = 100;
delta = rot_axis_angle([1;1;0], 0);
rpoints1 = delta * scan1(1:3, 1:every:tot);

R = rot_z(deg2rad(135)) * rot_y(deg2rad(25));

rpoints2 = R * delta * scan1(1:3, every/2:every:tot);


% Parameters for the HT
p = h3d_params;


if false
	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, rpoints1);
	ht2 = h3d_compute_cube_ht(p, base, rpoints2);
end

p.debug_true_R = R;

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


if false

	figure; hold on
	plot3points(rpoints1, 'b.');
	plot3points(Rest' * rpoints2, 'r.');

end


if false
	figure
	subplot(1,2,1); hold on;
	h3d_draw_sphere_hs(ht1u, 100);
	
	max_peak = max(p1u(1,:));
	min_peak = min(p1u(1,:));
	for i=1:3 %size(p1u,2)
		v = p1u(2:4,i);
		peak = p1u(1,i);
		len = 1.1 + 0.5*((peak-min_peak)/max_peak);
		p = plot3points([[0;0;0] len*v],'b-');
		set(p, 'linewidth', 2);
	end
	
	axis('equal');
		
	subplot(1,2,2); hold on;
	h3d_draw_sphere_hs(ht2u, 100);

	for i=1:3 %size(p1u,2)
		v = p2u(2:4,i);
		peak = p2u(1,i);
		len = 1.1 + 0.5*((peak-min_peak)/max_peak);
		p = plot3points([[0;0;0] len*v],'r-');
		set(p, 'linewidth', 2);
	end

	for i=1:3 %size(p1u,2)
		v = R' * p1u(2:4,i);
		peak = p1u(1,i);
		len = 1.1 + 0.5*((peak-min_peak)/max_peak);
		p = plot3points([[0;0;0] len*v],'b-');
		set(p, 'linewidth', 2);
	end
	

	axis('equal');


end