Our proposed solution combines point cloud registration with inertial estimation, forming a factor graph that simultaneously optimizes the pose and IMU biases. We developed a zero velocity detection module to ameliorate ICP drift when the agent is static and observes moving objects, as well as a dynamic motion detector to help the system to converge in the presence of aggressive motion. The robustness of our approach has been validated ...