Add KMeans Test
Made KMeans generic
Merge branch 'develop' of https://github.com/eluechin/KinectAutoCalibration into develop