A Temporal Perspective n-Point Problem with Model Uncertainties for Cooperative Pose Estimation in a Heterogeneous Robot Team

Many solutions exist for estimating the pose of an object with respect to a camera, where perfect knowledge of the object is assumed. In this work we lift the assumption of a perfectly known model and introduce uncertainties for the 3d points, which are retrieved from a dynamically created model. The positions of model points can either be uncorrelated or correlated. The latter is typically the case for mobile robots navigating based on results of visual-inertial pose estimation in unknown and GNSS-denied environments. In our approach, a selection of poses estimated by one robot is used as a dynamical 3d model and combined with 2d points from tracking the robot over time with the camera of another robot. In addition, selection criteria for adding and deleting 3d model points in an optimal way are proposed. Weighted residuals in the tangent space are used in a generalized least-squares problem to calculate the transformation between the tracking camera and an object. Measurement errors are projected into tangential planes of the unit sphere. The proposed method allows to estimate the relative pose of members of a robotic team with high accuracy. The benefits of our approach are shown in simulation and also during real-world experiments using visual odometry measurements from a multicopter that is tracked by the camera of a rover.