jly_sorting.hpp

/********************************************************************
Sorting/Selection functions for the Go-ICP Algorithm
Last modified: Jan 27, 2015

"Go-ICP: Solving 3D Registration Efficiently and Globally Optimally"
Jiaolong Yang, Hongdong Li, Yunde Jia
International Conference on Computer Vision (ICCV), 2013

Copyright (C) 2013 Jiaolong Yang (BIT and ANU)

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*********************************************************************/

#ifndef JLY_SORING_HPP
#define JLY_SORING_HPP

#define INTRO_K 5

#define INSERTION_NUM 5

//sorting in ascending order

template <typename T>
inline size_t median_of_st_mid_en(T * data, size_t st, size_t en)
{
	size_t mid = (st+en)/2;
	if(data[st] < data[en])
	{
		if(data[mid] < data[st])//median is data[st]
			return st;
		else if(data[mid] < data[en]) //median is data[md]
			return mid;
		else //median is data[en]
			return en;
	}
	else // data[en] <= data[st]
	{
		if(data[mid] < data[en])//median is data[en]
			return en;
		else if(data[mid] < data[st]) //median is data[md]
			return mid;
		else //median is data[st]
			return st;
	}
}

//median of 3 numbers
template <typename T>
inline size_t median_of_3(T * data, size_t st)
{
	T* data_ = data + st;

	if(data_[0] < data_[2])
	{
		if(data_[1] < data_[0])//median is data[0]
			return st;
		else if(data_[1] < data_[2]) //median is data[1]
			return st + 1;
		else //median is data[2]
			return st + 2;
	}
	else // data[2] <= data[0]
	{
		if(data[1] < data[2])//median is data[2]
			return st + 2;
		else if(data[1] < data[0]) //median is data[1]
			return st + 1;
		else //median is data[0]
			return st;
	}
}

//median of 5 numbers with 6 comparisons
template <typename T>
inline size_t median_of_5(T * data, size_t st)
{
	T* data_ = data + st;
	T tmp;

	if(data_[0] > data_[1])
	{
		tmp = data_[0];
		data_[0] = data_[1];
		data_[1] = tmp;
	}

	if(data_[2] > data_[3])
	{
		tmp = data_[2];
		data_[2] = data_[3];
		data_[3] = tmp;
	}

	if(data_[0] < data_[2])
	{
		tmp = data_[4];
		data_[4] = data_[0];

		if(tmp < data_[1])
			data_[0] = tmp;
		else
		{
			data_[0] = data_[1];
			data_[1] = tmp;
		}
	}
	else
	{
		tmp = data_[4];
		data_[4] = data_[2];

		if(tmp < data_[3])
			data_[2] = tmp;
		else
		{
			data_[2] = data_[3];
			data_[3] = tmp;
		}
	}

	if(data_[0] < data_[2])
	{
		if(data_[1] < data_[2])
			return st + 1;
		else
			return st + 2;
	}
	else
	{
		if(data_[0] < data_[3])
			return st;
		else
			return st + 3;
	}
}

template <typename T>
size_t median_of_medians(T * data, size_t st, size_t en)
{
	size_t l = en-st+1;
	size_t numof5 = l / 5;
	if(l % 5 != 0)
		numof5 ++;

	T tmp;
	size_t subst = st, suben = st + 4;
	size_t i, medind;

	//fist (numof5 - 1) groups
	for(i = 0; i < numof5 - 1; i++, subst += 5, suben += 5)
	{
		medind = median_of_5(data, subst);

		tmp = data[st+i];
		data[st+i] = data[medind];
		data[medind] = tmp;
	}

	//last group
	{
		switch(en-subst+1)
		{
		case 3: // 3 elements
		case 4: // 4 elements
			medind = median_of_3(data, subst);
			break;
		case 5: // 5 elements
			medind = median_of_5(data, subst);
			break;
		default: // 1 or 2 elements
			medind = subst;
			break;
		}

		tmp = data[st+i];
		data[st+i] = data[medind];
		data[medind] = tmp;
	}
	
	//median of medians
	if(numof5 > 5)
		return median_of_medians(data, st, st + numof5-1);
	else
	{
		switch(numof5)
		{
		case 3: // 3 elements
		case 4: // 4 elements
			return median_of_3(data, st);
			break;
		case 5: // 5 elements
			return median_of_5(data, st);
			break;
		default: // 1 or 2 elements
			return st;
			break;
		}
	}
}

template <typename T>
void insertion_sort(T * data, size_t st, size_t en)
{
	T tmp;
	size_t i, j;
	for(i = st+1; i <= en; i++)
		for(j = i; j > st && data[j-1] > data[j]; j--)
		{
			tmp = data[j-1];
			data[j-1] = data[j];
			data[j] = tmp;
		}
}

// Sort the given array in ascending order
// Stop immediately after the array is splitted into k small numbers and n-k large numbers
template <typename T>
void intro_select(T * data, size_t st, size_t en, size_t k)
{	
	T pivot;
	T tmp;

	//for(; st < en && data[st] > 0; st++);

	size_t l_pre = en-st+1;
	size_t l;
	size_t medind;

	bool quickselect = true;

	size_t i = 0;
	while(1)
	{
		if(st >= en)
			break;

		if(en - st <= INSERTION_NUM)
		{	
			insertion_sort(data, st, en);
			return;
		}

		if(quickselect && i++ == INTRO_K)
		{
			// switch to 'median of medians' if INTRO_K partations of quickselect fail to half the size 
			l = en-st+1;
			if(l*2 > l_pre)
				quickselect = false;
			
			l_pre = l;
			i = 0;
		}

		if(quickselect)
			//medind = st;
			medind = median_of_st_mid_en(data, st, en);
		else
			medind = median_of_medians(data, st, en);

		if(medind != st)
		{
			tmp = data[st];
			data[st] = data[medind];
			data[medind] = tmp;
		}

		size_t p = st;
		size_t left = st+1;
		size_t right = en;
		pivot = data[p];

		while(1)
		{
			while (left < right && pivot >= data[left])
				++left;
			while (left < right && pivot <= data[right])
				--right;

			if (left >= right)
				break;

			//swap left & right
			tmp = data[left];
			data[left] = data[right];
			data[right] = tmp;
		}

		size_t s = left-1;
		if(data[left] < pivot)
			s = left;
		//swap p & s
		data[p] = data[s];
		data[s] = pivot;

		if(s < k)
			st = s+1;
		else if(s > k)
			en = s-1;
		else //s == k
			break;
	}
}

#endif