A Novel Explicit Pose Estimation Algorithm based on Euclidean Geometry

In this paper, a novel explicit pose estimation algorithm using Euclidean geometry is presented. Based on feature point extraction using stereo cameras, the pose of arbitrary moving rigid body in 3D space is estimated in real-time. The merit of the algorithm proposed lies on the fact that, instead of estimating the entries of rotational matrix, the axis around which the rotation is made is estimated, which removes the constraints of other algorithms that estimated rotational matrix must be a special orthogonal one. Once the axis is obtained rotation angles and translation vectors can be solved straight forward. The algorithm is simple enough for realtime applications and can be directly applied for tracking and visual servoing. Simulation included shows the effectiveness of the algorithm.