% First part of the HSM algorithm: find the rotation(s) that
% overlap two point sets.

% We create the first set of points
	npoints = 180;
	xAxis = 2;
	yAxis = 1;
	noise = 0.1;
	points1 = patternEllipse(npoints,xAxis, yAxis, noise); 

% We sample a random roto-translation
	
	% translation
	T = rand(2,1);
	
	% rotation, between -pi and pi
	theta = (rand)*2*pi

% We do a roto-translation of the first set and then
% add some noise

	% rotation
	rotationMatrix = [cos(theta) -sin(theta); sin(theta) cos(theta)];
	points2 = rotationMatrix * points1;

	% and translation
	points2 = points2 + repmat(T,1,size(points2,2));
	
	% add noise
	noise = 0;
	points2 = points2 + noise * randn(2, size(points2,2));

% We compute the Hough Transform and Hough Spectrum for 
% the two sets of points

	% These are the parameters for HSM
	thetaCells = 180;
	rhoCells = 256;
	rhoScale = 2 * max(max(points2));

	ht1 = computeHoughTransform(points1, thetaCells, rhoCells, rhoScale);
	hs1 = computeHoughSpectrum(ht1, true);
	
	ht2 = computeHoughTransform(points2, thetaCells, rhoCells, rhoScale);
	hs2 = computeHoughSpectrum(ht2, true);
	
% To find an estimate of the rotation, we do a circular cross-correlation
% of the two spectra

	cross = circularCrossCorrelation(hs2,hs1, true);
	
	% the theta hypotheses are the maxima of the cross correlation
	% for now, let's extract only the highest maximum
	[notused, thetaEstimateCell1] = max(cross);
	% convert from cells to radians
	thetaEstimate1 = thetaEstimateCell1 * 2 * pi / thetaCells
	
	% this version of the algorithm gives an answer with an
	% ambuiguity of 180° (that is, you get. for example,
	% 41° and 221°
	thetaEstimateCell2 = mod(thetaEstimateCell1+thetaCells/2, thetaCells);
	thetaEstimate2 = thetaEstimateCell2 * 2 * pi / thetaCells
	 
% For comparison, we rotate back the second set by the theta estimate
	rotationMatrix = [cos(-thetaEstimate1) -sin(-thetaEstimate1); 
	                  sin(-thetaEstimate1) cos(-thetaEstimate1	)];
	points2back = rotationMatrix * points2;
	
% Display the result
	f = figure;
	
	% Data points
	subplot(4,1,1);
	hold on
	plot(points1(1,:),points1(2,:),'b.');
	plot(points2(1,:),points2(2,:),'g.');
	title 'Data points'
	axis image

	% The Hough Spectrum
	subplot(4,1,2);
	hold on
	plotHoughSpectrum(hs1, 'b');
	plotHoughSpectrum(hs2, 'g');
	title 'Hough spectrum'

	% The cross correlation
	subplot(4,1,3);
	hold on
	plotHoughSpectrum(cross, 'r');
	
	% The true theta (plotted in cells unit)
	plot(theta/(2*pi)*thetaCells,0.5,'kd');
	
	% The estimated theta (plotted in cells unit)
	plot(thetaEstimate1/(2*pi)*thetaCells,1,'rd');
	plot(thetaEstimate2/(2*pi)*thetaCells,1,'rd');
	
	title 'Spectra correlation'

	% Data points rotated back
	subplot(4,1,4);
	hold on
	plot(points1(1,:),points1(2,:),'b.');
	plot(points2back(1,:),points2back(2,:),'g.');
	title 'Result'
	axis image