Merge pull request #18371 from nathanrgodwin:sqpnp_dev

Merge pull request #18371 from nathanrgodwin:sqpnp_dev

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.