Mapping correction and drawKinectPoints() corrected