Fixes and optimizations for the SQPnP solver
* Fixes and optimizations
- optimized the calculation of qa_sum by moving equal elements outside the loop
- unrolled copying of the lower triangle of omega
- substituted SVD with eigendecomposition in the factorization of omega (2-3 times faster)
- fixed the initialization of lambda in FOAM
- added a cheirality test that checks a solution on all 3D points rather than on their mean. The old test rejected valid poses in some cases
- fixed some typos & errors in comments
* reverted to SVD
Eigen decomposition seems to yield larger errors in certain tests, reverted to SVD
* nearestRotationMatrixSVD
Added nearestRotationMatrixSVD()
Previous nearestRotationMatrix() renamed to nearestRotationMatrixFOAM() and reverts to nearestRotationMatrixSVD() for singular matrices
* fixed checks order
Fixed the order of checks in PoseSolver::solveInternal()
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.
