Pose Estimation with Dual Quaternions and Iterative Closest Point

This paper presents a method for pose estimation of a rigid body using unit dual quaternions where pose measurements from point clouds are filtered with a multiplicative extended Kalman filter (MEKF). The point clouds come from a 3D camera fixed to the moving rigid body, and then consecutive point clouds are aligned with the Iterative Closest Point (ICP) algorithm to obtain pose measurements. The unit constraint of the dual quaternion is ensured in the filtering process with the Dual Quaternion MEKF (DQ-MEKF), where the measurement updates are performed using the dual quaternion product so that the result is a unit dual quaternion. In addition, we use the Cayley transform for the discrete time propagation of the DQ-MEKF estimate, eliminating the need for normalization and projection of the resulting unit dual quaternion. The ICP algorithm is initialized with the time propagated state of the filter to give faster and more accurate pose measurements. To further improve the accuracy of the initialization, angular velocity measurements from a gyroscope fixed to the camera are included in the filter. The proposed method has been tested in experiments using a Kinect v2 3D camera mounted rigidly on a KUKA KR6 robotic arm, while a customized ICP algorithm was successfully implemented on a Graphical Processing Unit (GPU) system.