High-Accuracy Pose Estimation for 3D Reconstruction Considering the Manipulator Motion Constraints with an Eye-in-Hand RGB-D Camera

Three-dimensional(3D) environmental reconstruction of the manipulator workspace is a prerequisite to enable autonomous collision-free path planning and subsequent task execution, wherein the positioning accuracy of the equipped sensor is one key issue to determine the performance of 3D reconstruction. In this paper, an eye-in-hand RGB-D camera is used to complete the manipulator workspace scanning and a novel high-accuracy pose estimation method for 3D reconstruction is proposed, which not only considers the geometric and photometric constraints constructed from the visual information used in traditional methods, but also considers the manipulator motion constraints since the positioning accuracy of the manipulator is usually high. Based on above three constraints, three error functions are constructed respectively and fused with different weights to form an integrated objective function. Then, the problem is converted into solving the least squares problem by the Gaussian Newton nonlinear iterative method, which involves the computation of Jacobian matrices and residuals of the three error functions. To this end, the mathematical derivation of their formulas considering the manipulator motion constraints are given in detail. By qualitative and quantitative experimental comparison with traditional methods, the proposed method can greatly improve the accuracy of pose estimation and 3D reconstruction, which always maintains the error within 3 cm, and show good robustness even in the challenging environments with weak texture or geometric features. (Supplemented video link: https://youtu.be/FkFTxssnWms)