Bundle Adjustment is a least-squares objective function for reprojection errors. Researchers demonstrate that Bundle Adjustment can be implemented by Ceres and g2o and can be used for PnP pose estimation. In this question, I write a simple Gaussian Newton method , and use Bundle Adjustment to optimize the pose.

The whole demo is tested in Ubuntu Platorforms.

We know the information of 2D-3D corresponding points and use G-N to solve the pose.

Some related theories:Reprojection error

p2d.txt:Pixel coordinates in reprojection.
Data storageform:u v

p3d.txt:Map point in reprojection.
Data storage form:x y z

fx=520.9 fy=521.0 cx=325.1 cy=249.7

Sophus
We use Sophus for Lie groups commonly used for 2d and 3d geometric problems. Dowload and install instructions can be found at: https://github.com/strasdat/Sophus.

cd XX/XX(include GN-BA.cpp ,p2d.txt ,p3d.txt and CMakeLists.txt)  
mkdir build  
cd build  
cmake ..  
make -j2  
./GN-BA