Voxel grid filter optimisations

classic Classic list List threaded Threaded
6 messages Options
Reply | Threaded
Open this post in threaded view
|

Voxel grid filter optimisations

john24
This post was updated on .
Hello,

I've been using PCL in my research and found that few things have been rather slow, so I have spent some time to speed up the library and have gotten some promising results.

I would like to share them with the community.

First of all I found that the sorting method in include\pcl-1.7\pcl\filters\impl\voxel_grid.hpp was really slow:


So I have implemented a quick-sort method call which is quite a bit faster and gets the same results:


The code is not the pretiest but it sure has the performance:
void quickSortIterative(cloud_point_index_idx *cloud, cloud_point_index_idx *left, cloud_point_index_idx *right) {

	const int MAX_STACK = 400;//(right - cloud) + (left - cloud);
	int max = 0;
	int current = -1;
	cloud_point_index_idx ** required = (cloud_point_index_idx **)malloc(MAX_STACK * sizeof(cloud_point_index_idx*));

	required[++current] = left;
	required[++current] = right;
	while (current >= 0)
	{
		right = required[current--];
		left = required[current--];

		//cloud_point_index_idx* middle = ;
		unsigned int pivot = (cloud + (((right - cloud) + (left - cloud)) / 2))->idx;
		//unsigned int pivot = (left + (right - left))->idx;

		cloud_point_index_idx*ptrLeft = left, *ptrRight = right;
		uint64_t tmp;
		while (ptrLeft <= ptrRight)
		{

			while (*reinterpret_cast<unsigned int*>(ptrLeft) < pivot)
				ptrLeft++;

			while (*reinterpret_cast<unsigned int*>(ptrRight) > pivot)
				ptrRight--;

			if (ptrLeft <= ptrRight)
			{
				tmp = *reinterpret_cast<uint64_t*>(ptrLeft);
				*reinterpret_cast<uint64_t*>(ptrLeft++) = *reinterpret_cast<uint64_t*>(ptrRight);
				*ptrRight-- = *reinterpret_cast<cloud_point_index_idx*>(&tmp);
				/*
				tmp = ptrLeft->data;
				(*ptrLeft++).data = ptrRight->data;
				(*ptrRight--).data = tmp;
				*/
			}
		};


		if (left < ptrRight)
		{
			required[++current] = left;
			required[++current] = ptrRight;
		}
		if (ptrLeft < right)
		{
			required[++current] = ptrLeft;
			required[++current] = right;
		}
	}
	free(required);
}

void quickSortIterative(std::vector<cloud_point_index_idx> &cloud)
{
	quickSortIterative(cloud._Myfirst, cloud._Myfirst, cloud._Mylast);
}

I've made a lot of other changes as well which overall gave me an average of 80 times better performance. If anyone is interested I will share rest of the code.

I also optimised the OrganisedConversion<>::convert() and the pass-through filter
Reply | Threaded
Open this post in threaded view
|

Re: Voxel grid filter optimisations

Jochen Sprickerhof
Administrator
Hi John,

Interesting results. You can find documentation on how to contribute to
PCL here:

https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md

Regarding the sorting issue, Can you post some raw numbers about the
size of the vector and some testing data?

Cheers Jochen

_______________________________________________
PCL-developers mailing list
[hidden email]
http://pointclouds.org/mailman/listinfo/pcl-developers
http://pointclouds.org

signature.asc (836 bytes) Download Attachment
Reply | Threaded
Open this post in threaded view
|

Re: Voxel grid filter optimisations

john24
Yes sure, for my test data I used is the new tsukuba stereo dataset (provides you with 1800 stereo images pairs, depth maps etc.):

Depth:


Colour:



I used the OrganisedConversion<>::convert() to create point clouds out the depth and colour images, and then voxel grid down sample them.

The point size is: 640 x 480 = 307200 points per point cloud.

I was able to get converting depth+colour images to point clouds -> apply pass through filter -> voxel down-sample (leaf size 0.03f, 0.03f, 0.03f) the point cloud and rendering it all to screen at average of 25fps, while before optimising the code I could only get 0.4fps.

I'll try and get the rest of the code for other modules uploaded here first for you to double check before I contribute to the repository as I am not good with github and such.
Reply | Threaded
Open this post in threaded view
|

Re: Voxel grid filter optimisations

john24
In reply to this post by Jochen Sprickerhof
Also got a spreadsheet summary of the optimisation performance, this show how much of the CPU cycles each part used out of a 100%. This is after organised conversion was optimised.

 
Reply | Threaded
Open this post in threaded view
|

Re: Voxel grid filter optimisations

VictorL
In reply to this post by john24
john24 wrote
I'll try and get the rest of the code for other modules uploaded here first for you to double check before I contribute to the repository as I am not good with github and such.
Hello,

About that, don't hestitate to ask for help!
Create a pull request and play with it if you need to; there's no harm doing that.

Bye
Reply | Threaded
Open this post in threaded view
|

Re: Voxel grid filter optimisations

john24
Thank you :)

Well I just made a github account and got the tool downloaded, forked the repository and cloned it.

Need to do more research into using github properly.

I will be needing some help as the code changes I've made are super fast but not the easiest to read.