Pose-estimation-using-4-co-planar-points

Traditional PnP algorithms may not perform well when the points selected are co- planar. However, in the robotics challenge, for the problem of identifying the pose of the counter top, the easiest points to identify are the corners of the counter top. Hence, we decided to use a method proposed by Zhang et al. [3]. In this paper they present a pose estimation algorithm based on four co-planar point correspondences. Given four co-planar points and their corresponding image points under a perspective camera, plus the camera’s intrinsic matrix, the camera’s rotation and translation relative to the object plane is determined directly. In essence, the pose estimation problem is converted to the calculation of a planar homography between the object plane and the image plane. Experiments with both synthetic data and real images verify the correctness of this algorithm. The authors do not provide a downloadable implementation. Hence we had to do implement by ourselves and test it.