Simultaneous Estimation of Target Pose and 3-D Shape using the FastSLAM Algorithm

A solution is presented to the problem of estimating recursively the 6-DOF pose and 3-D shape of a target, using insights from the SLAM community. The algorithm in this paper is referred to as ‘SLAM-inspired Pose Estimation and Reconstruction’ (or ‘SPEAR’), and is based on the FastSLAM algorithm. In particular, FastSLAM’s use of Rao-Blackwellized particle lters is emulated. SPEAR tracks the target with measurements from cameras; this paper focuses on the monocular case. No a priori knowledge of the target is required. This paper discusses the end-to-end implementation of SPEAR in detail, including the algorithm itself, feature initialization steps, and the image processing step (which uses SIFT). Also, specics are given on what is needed computationally to perform SPEAR in real-time. The rationale for applying Rao-Blackwellized particle lters is discussed. Compared with a solely EKF-based approach to solving SPEAR, particle lters allow for more ecient measurement updates and are not constrained by the assumption of unimodal probability distributions. Results are given for several targets in the lab and in the eld, demonstrating successful estimation of each target’s pose along with the 3-D locations of features on the body.