Coupled 3D Tracking and Pose Optimization of Rigid Objects Using Particle Filter

In order to track and estimate the pose of known rigid objects with high accuracy in unconstrained environment with light disturbance, scale changes and occlusion, we propose to combine 3D particle filter (PF) framework with algebraic pose optimization in a closed loop. A new PF observation model based on line similarity in 3D space is devised and the output of 3D PF tracking, namely line correspondences (model edges and image line segments), are provided for algebraic line-based pose optimization. As a feedback, the optimized pose serves as a particle with high weight during re-sampling. To speed up the algorithm, a dynamic ROI is used for reducing the line detection and search space. Experiments show our proposed algorithm can effectively track and accurately estimate the pose of freely moving 3D rigid objects in complex environment.