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()
8d0fbc6a1e