The cpp implementation of the CPnP solver is available at https://github.com/bereze/CPnP-cpp-version.

This is the description of the use of the CPnP solver. Paper information:

@inproceedings{zeng2023cpnp,
  title={CPnP: Consistent Pose Estimator for Perspective-n-Point Problem with Bias Elimination},
  author={Zeng, Guangyang and Chen, Shiyu and Mu, Biqiang and Shi, Guodong and Wu, Junfeng},
  booktitle={IEEE International Conference on Robotics and Automation (ICRA)},
  pages={},
  year={2023},
  organization={IEEE}
}

One can directly run main.m to test the CPnP solver. The main.m mainly includes three parts:

• Generate input data: generate the intrinsics of the camera, the pose of the camera, the coordinates of 3D points, and the noisy 2D projections. This part uses the codes from Copyright (C) <2007> <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>. Reference paper: Moreno-Noguer F, Lepetit V, Fua P. Accurate Non-Iterative 𝑂 (𝑛) Solution to the P𝑛P Problem[C]//2007 IEEE 11th International Conference on Computer Vision. IEEE, 2007: 1-8.
• Estimate the pose of the camera: estimate the camera pose using CPnP function and calculate the rmse
• Calculate the CRB: calculate the theoretical lower bound for the rmse of any unbiased PnP solver

The inputs and outputs of the CPnP function are as follows:

• Inputs

s - a 3×n matrix whose i-th column is the coordinates (in the world frame) of the i-th 3D point

Psens_2D - a 2×n matrix whose i-th column is the coordinates of the 2D projection of the i-th 3D point

fx, fy, u0, v0 - intrinsics of the camera, corresponding to the intrinsic matrix K=[fx 0 u0;0 fy v0;0 0 1]

Remark: the units of Psens_2D, fx, fy, u0, and v0 should be consistent, e.g., all in m, or all in pixels.

• Outputs

R - the estimate of the rotation matrix in the first step

t - the estimate of the translation vector in the first step

R_GN - the refined estimate of the rotation matrix with Gauss-Newton iterations

t_GN - the refined estimate of the translation vector with Gauss-Newton iterations