Accurate fiducial mapping for pose estimation using manifold optimization

The accurate pose estimation for moving objects within a given workspace is one of the most fundamental tasks for many applications including augmented reality, robotics' control, planning and navigation. The information of objects' pose is often given by motion capture systems and global positioning systems indoor and outdoor respectively. However, motion capture systems are costly and limited in workspace, while global positioning systems degrade severely in clustering environments. In this paper, we propose an approach to build a map of fiducial markers based on manifold optimization and then extend the fiducial map for pose estimation. The fiducial map based pose estimation system is cost-effective, lightweight and can work both indoor and outdoor. The proposed method starts by fiducial detection and pose estimation for collected images in order to establish an initial graph which stacks measurements of markers' relative poses. Then for each relative pose, multiple measurements are fused using manifold optimization for an optimal estimation. To deal with pose ambiguity problem, inlier poses are selected using the random sample consensus algorithm. Finally, a global pose optimization is done on manifold to minimize per frame reprojection errors. Mapping experiments with synthetic and real data demonstrated the accuracy and consistency of the proposed approach. The accuracy of pose estimation using prebuilt fiducial map was evaluated by benchmark tests with motion capture system.