Towards realistic covariance estimation of ICP-based Kinect V1 scan matching: The 1D case

The Iterative Closest Point (ICP) algorithm is a classical approach to obtaining relative pose estimates of a robot by scan matching successive point clouds captured by an onboard depth camera such as the Kinect V1, which has enjoyed tremendous popularity for indoor robotics due to its low cost and good performance. Because the sensed 3D point clouds are noticeably corrupted by noise, it is useful to associate a covariance matrix to the relative pose estimates, either for diagnostics or for fusing them with other onboard sensors by means of a probabilistic sensor fusion method such as the Extended Kalman Filter (EKF). In this paper, we review the sensing characteristics of the Kinect camera, then present a novel approach to estimating the covariance of pose estimates obtained from ICP-based scan matching of point clouds from this sensor. Our key observation is that the prevailing source of error for ICP registration of Kinect-measured point clouds is quantization noise rather than white noise. We then derive a closed-form formula which can be computed in real time onboard the robot's hardware, for the case where only 1D translations are considered. Experimental testing against a ground truth provided by an optical motion capture system validates the effectiveness of our proposed method.