Clean up code and added kinect markers