Orientation correction for Hector SLAM at starting stage

Hector simultaneous localisation and mapping(SLAM) is a popular approach for mapping a space. It requires only a Light Detection and Ranging (LiDAR) sensor to perform the mapping. It uses previous scan results to estimate the current state of the system. However, Hector SLAM suffers from serious drifting in the starting stage. This does not affect the mapping during the process but will significantly interfere the future pose estimation of the robot. Because the future pose is an estimation from the previous pose, the drift from the beginning will be recorded and results in a random rotation and translation of the map frame against other ground truth frames. This research uses a reference frame to locate the robot and correct its orientation and position during the starting period of Hector SLAMing using Point-Line Iterative Closest Point (PL-ICP). By compare the trajectory from the reference frame and the trajectory generated by the Hector SLAM, the translations and rotations caused by the joggling from the beginning can be estimated. Map and current poses of the Hector node are rotated and translated according to this translation and rotation to re-align the mapping frame to the ground truth frame.