Slow convergence of `solvePnPRefineLM`

Python
,
1

I use solvePnPRefineLM function to optimize camera pose based on 3 pairs of 3D2D correspondences. It works well in most cases. However, in one case, it converges too slow, even sets max_iter to 100000, it does not reach an accuracy of 1e-6.
I have checked the source code of LMSolver (levmarg.cpp), I found that it adds iter even when the optimization target is not updated (only update lambda).
I wonder if it is better to only add iter when the optimization target is updated.

2

outliers in the data?

also, that epsilon sounds quite tight. I don’t know this function so I can’t judge. it’s just intuition.

3

Thanks for the reply. There is no outlier, they are just three corners of a paper on the floor. Yes, epsilon=1e-6 is tight. However, in most cases, it can be reached in default 20 iterations.
With checking the source code, I think most iterations are used to update lambda in LMSolver. Thus, I wonder if counting iterations like this is proper.

4

Just for curiosity, did you try to compare the results between solvePnPRefineLM() and solvePnPRefineVVS()?
I am curious to know if one method can converge while the other not and vice versa.

I would try to plot the solutions or the residuals along with the iteration number to try to understand what is going on under the hood.

For instance, maybe it is oscillating and could explain the high number of iterations needed to converge at an accuracy of 1e-6?

Also, pose estimation is a non-linear problem. Maybe if the initial solution is not close to the true one, it is diverging and could explain the high number of iterations?

5

Thanks for the reply. To help reproduce the problem. I attach the piece of code here. You can use the data directly to reproduce the problem. With experiments you can find that, solvePnPRefineVVS() can converge within default 20 cycles, but solvePnPRefineLM() converge much slower. To double confirm this initial condition is able to converge, I also implemented the Gauss-Newton to test. It can also converge fast. Theoretically, LM should converge faster than Gauss-Newton generally.

import numpy as np 
import cv2



def project(pts_3d, K, R, t):
    pts_homogeneous = K @ (R @ pts_3d.T + t[:, np.newaxis])
    pts_homogeneous = pts_homogeneous.T
    pts_2d = pts_homogeneous[:, :2] / pts_homogeneous[:, 2:]
    return pts_2d



pts_3d = np.asarray([[ 0.1485, -0.1055,  0.],
                     [-0.1485, -0.1055,  0.],
                     [ 0.1485,  0.1055,  0.]])

pts_2d = np.asarray([[952.35820507, 442.57192054],
                     [702.10139942, 855.32811759],
                     [669.07345369, 208.71272889]])

K = np.asarray([[1573.50867, 0.        , 962.900820],
                [0.        , 1573.50867, 716.048100],
                [0.        , 0.        , 1.        ]])

T0 = np.asarray([[ 0.46682, -0.74434,  0.47754, -0.1798174 ],
                 [-0.83228, -0.55233, -0.04733, -0.08446406],
                 [ 0.29899, -0.37535, -0.87734,  0.88700609],
                 [ 0.     ,  0.     ,  0.     ,  1.        ]])

R, t = T0[:3, :3].copy(), T0[:3, 3].copy()
error0 = (project(pts_3d, K, R, t) - pts_2d).flatten()
r, _ = cv2.Rodrigues(R)
criteria = (cv2.TERM_CRITERIA_MAX_ITER+cv2.TERM_CRITERIA_EPS, 20, 1e-6)
r, t = cv2.solvePnPRefineLM(pts_3d, pts_2d, K, None, r, t, criteria=criteria)
# r, t = cv2.solvePnPRefineVVS(pts_3d, pts_2d, K, None, r, t, criteria=criteria)
T = np.identity(4)
R, _ = cv2.Rodrigues(r)
error = (project(pts_3d, K, R, t) - pts_2d).flatten()
print(error0)
print(error)
6

Thanks for the reproducing code.

Could you add also your custom Gaussian-Newton optim code?

7

I am so sorry that I cannot attach my Gauss-Newton code here because it depends on other Lie algebra code I developed for the company. However, it is just standard optimization on SE(3).