- fixed need of color vector
- cleaned up mess in tsd_kinect application
- speeded up ICP