% Performs Hough Scan Matching (HSM). 
% The file is long but (I hope) well commented. Don't esitate to contact me 
% if you have any problem.
%
% There are a lot of params, listed here in order of 
% importance. The function returns a rich structure "result" 
% where all the computations are registered. You can
% display nicely the result with the function displayHSMResult().
%
%	The two sets of points:
%		params.points1 
%		params.points2 
%
%	The parameters for the Hough Transform:
%		params.thetaCells 
%		params.rhoCells 
%		params.rhoScale            real-world width of buffer 
%
%	Parameters that characterize the search domain:
%		params.maxTBound          bound for translation (|T|<maxTBound)
%
%	Parameters for HSM:
%		params.maxPhiHypotheses     max number of hypotheses for phi
%		params.maxTHypotheses     max number of T hypotheses for each phi hypothesis
%		params.maxRhoCorrelations   max number of correlations for
%		                            finding the translation
%	The following are parameters used to work around noise on the buffer
%		params.phiFilterSigma         low-pass filter (cells)
%		params.rhoFilterSigma         low-pass filter for T buffer (cells)
%


function result = doHSM(p)
	
	% First we compute the Hough Transform 
		result.ht1 = computeHoughTransform(p.points1, p.thetaCells, p.rhoCells, p.rhoScale);
		result.ht2 = computeHoughTransform(p.points2, p.thetaCells, p.rhoCells, p.rhoScale);

	% Then we compute the Hough Spectrum
		result.hs1 = computeHoughSpectrum(result.ht1, true);
		result.hs2 = computeHoughSpectrum(result.ht2, true);
		
	% To find an estimate of the rotation, we do a 
	% circular cross-correlation of the two spectra
		result.crossCorrelation = circularCrossCorrelation(result.hs2, result.hs1, true);
	
	% We then do simple low-pass filtering to eliminate noise
		result.crossCorrelationFiltered = simpleLowPassFilterCircular(result.crossCorrelation, p.phiFilterSigma);
		
	% We then extract the maxima of the filtered cross correlation:
	% these are our hypotheses for the rotation
	result.phiIndexes = findLocalMaximaCircular(result.crossCorrelationFiltered, p.maxPhiHypotheses);	
	result.phiHypotheses = result.phiIndexes * 2 * pi / p.thetaCells
	
	% Now we find a translation for each rotation hypothesis
			
	for i=1:size(result.phiHypotheses,2)
		phi=result.phiHypotheses(i);
		
		% Saving debug info into this structure
			r=struct();

		% Assuming phi, we rotate the second spectrum
			% radians->cells
			phiCell = phi / (2*pi) * p.thetaCells;
			% circular rotation
			r.hs2rot = circularRotation(result.hs2, phiCell);
		
		% We have to choose the columns (=which directions) of the HT to correlate
		% the best way to choose is to use the maxima of the spectrum;
		% therefore (to suppress noise) we multiply the two spectra, filter
		% and get the maxima
			r.product = result.hs1 .* r.hs2rot;
			r.productFiltered = simpleLowPassFilterCircular(r.product, p.phiFilterSigma);
			
		% Because the spectrum is 180-periodic, we search for maxima only in the first half
			r.corrIndexes = findLocalMaxima(r.productFiltered(1:floor(p.thetaCells/2)), p.maxRhoCorrelations);	
		% We need at least 2 directions
			if (size(r.corrIndexes,2)==1)
				r.corrIndexes(2)  =  round(r.corrIndexes(1)+p.thetaCells/4); 
			end
			
		% Now we setup a buffer, which will hold the prob. distribution for T  
			cells = round(p.maxTBound * (p.rhoCells/p.rhoScale));
			r.tbuffer = zeros(cells*2+1,cells*2+1);
			
		% For each direction
		for j=1:size(r.corrIndexes,2)
			index=r.corrIndexes(j);
			direction=index*2*pi/p.thetaCells;
			
			% We extract the two columns of the HT
				col1 = result.ht1(index,:);
				% we must "rotate" by phi
				index2 = round(mod(index+phiCell, p.thetaCells));
				col2 = result.ht2(index2,:);
				
			% We do a cross correlation between the two columns
				crossCorrelationRange = cells; 
				crossCorrelation = xcorr(col2,col1,crossCorrelationRange);
			
			% Now for each element in the T buffer,
			% we increase the value by this observation
			for a=1:size(r.tbuffer,1)
			for b=1:size(r.tbuffer,2)
				% Which value of the translation T is associated to this cell?
				T = [ (a-cells) (b-cells) ]' * p.rhoScale / p.rhoCells;
				
				% We project T on the correlation direction
				pT = [cos(direction) sin(direction)] * T;
				
				% We transform the projection to cells of the correlation 
				% buffer
				pTcell = round(pT * p.rhoCells / p.rhoScale + cells);

				if (pTcell>=1) && (pTcell<=size(crossCorrelation,2))
					value = crossCorrelation(pTcell);
					r.tbuffer(a,b)=r.tbuffer(a,b)+value;
				end
			end
			end
		end % For each direction
		
		% Now the buffer is complete, to find the T we extract the maxima
		% (for simplicity, we use only one maximum)
		r.tbufferFiltered = simpleLowPassFilter2(r.tbuffer, p.rhoFilterSigma);
		r.Tcells = findLocalMaxima2(r.tbufferFiltered, p.maxTHypotheses);
		num=size(r.Tcells,2);
		% Convert back to world-coordinates
		r.THypotheses = (r.Tcells-repmat([cells cells]',1,num)) * p.rhoScale / p.rhoCells;
		
		% Saving debug info into main structure
		result.r(i)=r;
	end

		
	result.params=p;