Refactored code more. Now the cluster extraction code is essentially ready to be integrated into kinfu (minus the bounding box publishing).
I believe I've got the gaussian publishing working correctly. Still have to clean up debugging code.