Kinect code added, check vision.cpp for old code (commented out)