Rigid Point Set Registration Based on Cubature Kalman Filter and Its Application in Intelligent Vehicles

Point set registration is a key problem in intelligent vehicle localization and mapping. This paper presents a rigid point set registration algorithm based on the cubature Kalman filter (CKF). First, the point set registration problem is cast into the state space model assuming that the correspondence between these two point sets is previously unknown. Then, CKF is used to solve this nonlinear filtering problem. At every iterative step, all the points in the moving point set will be considered, and the corresponding points in the model point set will be updated accordingly. In the time update, the scale of the free space that can be explored is significant for this registration algorithm. Herein, continuous simulated annealing (CSA) is adopted to gradually optimize the covariance of the model noise to speed up convergence. Tests on public data sets show that the CKF-based point set registration algorithm is robust to outliers, noise, and initialization misalignment. Then, the application of this algorithm on intelligent vehicles and intelligent transportation systems is demonstrated through mapping and localization experiments. The precision and robustness are both validated compared with the traditional ICP, NDT, and CPD based point set registrations in the localization experiments. Thus, the contributions of this paper are threefold. First, a CKF scheme is utilized in the point set registration problem. Second, CSA serves as the local optimizer to speed up the convergence process and make it more accurate. Third, this filtering-based method is adopted in intelligent vehicles and SLAM applications.