Review#4:Camera Pose Estimation using PnP

Sep 24 2019 Robotics

Prerequisite Knowledge


ArUco is a kind of encoding code similar to QR code. It’s commonly used in Augmented Reality.

Example of markers images

Camera Calibration

Intrinsic Parameter

PnP Algorithm

The PnP (Perspective n-Point) algorithm takes n points in the 3D world and their corresponding 2D points in the image as inputs, and then calculates the pose and position of the camera relative to the world. (NOTE: Chosen points cannot be colinear)

RANSAC might be useful for removing outliers.


System Design

I used 4 ArUcos just for averaging the result. The algorithm will work if it’s just one ArUco. The 4 corner points of a certain ArUco can be treated as feature points. I defined the center of 4 ArUcos as the origin (0, 0) of the world, then we know the 3-D coordinates of 4 corner points. After we detect the ArUco in the images, we will know their coresponding 2-D coordinates in the images. So, we can solve and find the translation and rotation vectors using the equation above. Here, translation and rotation vectors are the so-called camera pose (orientation and position).

Finally, I used MoveIt! library in ROS framework to control a UR5 robot, which I installed a camera. Keeping moving the robot, its pose (same as the camera pose) will change, then I can verify my algorithm compare to the pose data in the robot control panel.

Actually, this is a review section. I have done this project a year ago, so I forget the accuracy data from my research. I just remember this algorithm worked well.

OpenCV Function