Added SQPnP algorithm to SolvePnP
* Added sqpnp
* Fixed test case
* Added fix for duplicate point checking and inverse func reuse
* Changes for 3x speedup
Changed norm method (significant speed increase), changed nearest rotation computation to FOAM
* Added symmetric 3x3 inverse and unrolled loops
* Fixed error with SVD
* Fixed error from with indices
Indices were initialized negative. When nullspace is large, points coplanar, and rotation near 0, indices not changed.
Nathan Godwin
2255973b0f