@nathanrgodwin Thanks for the contribution.
@vpisarev Please review the patch.

interesting paper
@nathanrgodwin did you check if the latest two commits at the original repo are relevant for adding them here?

@tompollok I have the changes from the new commit "New InvertSymmetric3x3" added locally but I haven't added the loop unrolling from their latest commit yet. It should be trivial to make that change. I haven't had the chance to test these changes yet, but once I do, I'll add them to the PR.

@vpisarev is there a chance for this pr to still make it for 4.5 release? It does not alter any previous functionality and seems like a very interesting pnp solver.

I'm interested in testing sqpnp algorithm, so for that I implemented the code sqpnp.h and sqpnp.cpp into aruco library to compare the performance of sqpnp and IPPE which is the algorithm implemented there.

Here you can find aruco with sqpnp implemented and also a real video of markers given by aruco
https://github.com/JaouadROS/aruco-3.1.12

I didn't have to do much modifications to get it to work with aruco so it is fairly simple to see what I've modified.

I've noticed that the current implementation of sqpnp is unstable, very often giving a completely wrong results. For your information Aruco is using the four planar corners of the marker to estimate the pose. I don't know if this is a limitation of sqpnp but I should investigate that when I finish reading the article of sqpnp. My experience with IPPE is that it is only suffering from pose ambiguity in some situations, otherwise it is always giving the right solution.

aruco-3.1.12

Please note, that it is under GPLv3, so it's derived work can't be used in OpenCV.

Yes I agree but this is just for a purpose of testing opencv (sqpnp algorithm) in aruco for a real application and not the way around. Thanks for pointing that out!

I've noticed that the current implementation of sqpnp is unstable, very often giving a completely wrong results.

@JaouadROS This is somewhat surprising since the SQPnP code passes the same unit tests that the OpenCV IPPE implementation passes as well as a number of synthetic tests developed by the paper's authors (these tests are used in my own repo https://github.com/nathanrgodwin/SQPnP). I'll take a look at your implementation and see if I can recreate the issue.

@JaouadROS Thank you for bringing this bug to my attention.

The error stemmed from the SVD operation. The solver computes U and Vt matrices sufficient to reproduce the original matrix. Because the PSD matrix being decomposed was not full rank, when one of the values of the vectors of the orthogonal basis of the nullspace in Vt reaches 0, the corresponding value in U is no longer contributes to reproducing the original matrix and can't be optimized further. Switching to use Vt instead of U for the eigenvectors resolved the problem.

After resolving this error and running approximately 2000 solving iterations on your aruco marker video, SQPnP produced a re-projection error of more than 2 only once. Although it seems in this case, IPPE outperformed SQPnP, possibly because the points here are coplanar.

AVG SQPNP RMSE: 0.364983
AVG IPPE RMSE: 0.281046

Thank you @nathanrgodwin for the quick reply and debug, now it works as good as IPPE in the case of four planar points with slightly better performance for IPPE. Still though both suffer from ambiguity and it is something that can't be detected through the projection error.

Now I've another case in which I don't get the correct results. This time I've 11 points non-coplanar and I compare the pose between two frames with a very small movement of the points (you can see that when you plot the image points) and still the poses are very different.
PS: I can't use IPPE to compare with as the points are not planar.
Here is the code where I reported the 3D and 2D points (normalized):

if(sqpnp)
{
    std::vector<cv::Point3f> objectspoints =  {
    cv::Point3f(-41.851852, 41.851852, -16.842091),
    cv::Point3f( -20, 41.851852, -3.8461537),
    cv::Point3f( 20, 41.851852, -3.8461537),
    cv::Point3f( 41.851852, 41.851852, -16.842091),
    cv::Point3f( 41.851852, 12.962963, -16.842091),
    cv::Point3f( 41.851852, -19.25926, -16.842091),
    cv::Point3f( 41.851852, -41.851852, -16.842091),
    cv::Point3f( 0, -41.851852, 0),
    cv::Point3f( -41.851852, -41.851852, -16.842091),
    cv::Point3f( -41.851852, -8.8888893, -16.842091),
    cv::Point3f( -41.851852, 23.333334, -16.842091)};
    
    //Image 1
    std::vector<cv::Point2f> imagepointsOne =  {
    cv::Point2f(1137.9016, 950.77344),
    cv::Point2f(1159.5182, 949.81732),
    cv::Point2f(1203.4036, 948.90002),
    cv::Point2f(1224.5916, 949.11609),
    cv::Point2f(1225.0487, 981.2159D4),
    cv::Point2f(1225.9895, 1016.6479),
    cv::Point2f(1226.1592, 1042.022),
    cv::Point2f(1183.2811, 1043.1893),
    cv::Point2f(1139.5818, 1043.92),
    cv::Point2f(1138.9783, 1007.2895),
    cv::Point2f(1138.0909, 971.94623)};
    
    //Image 2
    std::vector<cv::Point2f> imagepointsTwo =  {
    cv::Point2f(1134.5905, 950.98566),
    cv::Point2f(1155.4135, 949.81573),
    cv::Point2f(1199.0874, 948.9024),
    cv::Point2f(1221.0641, 948.86908),
    cv::Point2f(1221.9208, 981.15881),
    cv::Point2f(1222.5052, 1016.6299),
    cv::Point2f(1223.0107, 1042.0221),
    cv::Point2f(1179.0441, 1043.0754),
    cv::Point2f(1136.1112, 1043.9739),
    cv::Point2f(1135.3933, 1007.2523),
    cv::Point2f(1134.8104, 972.00061),
    cv::sqpnp::PoseSolver solver;
    solver.solve(objectspoints, imagepointsOne, vec_rvecs, vec_tvecs);
    std::cout<<"Tvec: "<<vec_tvecs[0].t()<<"\nDistance: "<<cv::norm(vec_tvecs[0])<<std::endl;
    std::cout<<"Rvec: "<<vec_rvecs[0].t()*(180.f/M_PI)<<std::endl;
}

@JaouadROS The results for these point sets do produce bad poses, however, the poses produced are the same as the author's implementation. OpenCV's SOLVEPNP_ITERATIVE also produces a high reprojection error.

@nathanrgodwin Thank you for your time. I'll double-check those points again and I'll let you know. But for SOLVEPNP_ITERATIVE, it needs a good initial guess (it works with useExtrinsicGuess sets to true) so it won't converge without a
good guess given as input. I think, internally, it is just an LM optimizer.

@JaouadROS Yeah, it is LM internally. For LM, it is possible for it to converge without a good guess, however, since it finds a local minimum, convergence without a good guess does depend on the convexity of the solution space.

I mostly provided this as a quick sanity check, if this converged with LM without extrinsic guess, there would clearly be a problem with the SQPnP solution. You could try the other relevant OpenCV PnP solvers as well with your sample points to determine if the point sets can produce a valid solution.

@nathanrgodwin, thank you for the contribution! 👍