Hector SLAM with ICP Trajectory Matching

Simultaneous Localization and Mapping (SLAM) technologies are capable of mapping complicated environments nowadays. However, most of the existing sensor fusion approaches could not recover the system from significant failures. These failures may cause by an unexpected move of the robot, rapid change of the surrounding environment or other sensor degradation scenarios, such as poor lighting condition and smoke. During these scenarios, a SLAM system will likely generate misleading pose estimation. The accumulation of drifting will eventually cause map deformations. In this article, we propose a trajectory matching algorithm to help Hector SLAM improve its error accumulation problem. The proposed method uses Iterative Closest Point (ICP) and a reference frame to evaluate the drifting of the system. It corrects both the current system pose and existing mapping results. The proposed method is evaluated and discussed using both publicly available dataset and experiments.