A simple, easy to understand p3p solver with state of the art performance. http://openaccess.thecvf.com/content_ECCV_2018/html/Mikael_Persson_Lambda_Twist_An_ECCV_2018_paper.html
This is the benchmark code.
A corresponding pnp solver is found in: https://github.com/midjji/pnp
For a wrapper around the p3p specifically look to: https://github.com/midjji/pnp/blob/master/p4p.h