New p3p algorithm (accepted by CVPR 2017) (#8301)

* add p3p source code

* indent 4

* update publication info

* fix filename

* interface done

* plug in done, test needed

* debugging

* for test

* a working version

* clean p3p code

* test

* test

* fix warning, blank line

* apply patch from @catree

* add reference info

* namespace, indent 4

* static solveQuartic

* put small functions to anonymous namespace