Unstable solvePnP in Iterative mode

edit

Hi,

we currently investigating really unstable results when applying solvePnP on our parameters. Under certain circumstances the complete translation vector is flipping into the wrong direction when slight changes to the focal point is applied.

Our minimal python example:

import numpy as np
import cv2 as cv
import platform

print('numpy:', np.__version__)
print('opencv:', cv.__version__)
print('architecture:', platform.architecture())


class Vector2d:    
    def __init__(self, X=0, Y=0):
        self.X = X
        self.Y = Y

def GetIntrinsicParametersMatrix(focalLength, principalPoint):
    return np.array([
        [focalLength.X,             0, principalPoint.X ],
        [            0, focalLength.Y, principalPoint.Y ],
        [            0,             0,                1 ]
    ], dtype=np.float32)

def GetDistortionCoefficients(calibrationData):        
    Radial = calibrationData['Radial']
    Tangential = calibrationData['Tangential']

    if (Radial[3] != 0 or Radial[4] != 0 or Radial[5] != 0):
        return np.array([Radial[0], Radial[1], Tangential[0], Tangential[1], Radial[2], Radial[3], Radial[4], Radial[5]], dtype=np.float32)
    elif Radial[2] != 0:
        return np.array([Radial[0], Radial[1], Tangential[0], Tangential[1], Radial[2]], dtype=np.float32)
    else:
        return np.array([Radial[0], Radial[1], Tangential[0], Tangential[1]], dtype=np.float32)


if __name__ == '__main__':
    imagePoints = [
        [2590.57153, 1270.71423],
        [2233.14282, 1032.85718],
        [2945.71436, 1034.28577],
        [1749.85718, 2218],
        [3423.85718, 2219.85718]
    ]
    modelPoints = [
        [0, 15, 5.75],
        [-23.5, 0, 0],
        [23.5, 0, 0],
        [-56.2, 78, 11.9],
        [56.2, 78, 11.9]
    ]

    calibrationData = { 
        'Radial': [ -0.0523676442172271, -0.335562126146013, 2.63059804821136, 0.0523676442003532,0.33556208204044, -2.63062713657836 ],
        'Tangential' : [ 0, 0 ] 
    }
    distortionCoefficients = GetDistortionCoefficients(calibrationData)

    ppNormal = Vector2d(2591.661, 1728.591)
    ppStrange = Vector2d(2591.66102211408, 1728.59081758821)
    focalLength = Vector2d(8343.64179030465, 8344.73016777231)

    # intrinsicMatrix = GetIntrinsicParametersMatrix(focalLength, ppNormal)
    intrinsicMatrix = GetIntrinsicParametersMatrix(focalLength, ppStrange)

    rvec = np.zeros([3, 1], dtype=np.double)
    tvec = np.zeros([3, 1], dtype=np.double)

    retval, rvec, tvec = cv.solvePnP(
        objectPoints = np.array(modelPoints, dtype=np.double), 
        imagePoints = np.ascontiguousarray(np.array(imagePoints, dtype=np.double)[:,:2]).reshape((len(imagePoints),1,2)), 
        cameraMatrix = intrinsicMatrix, 
        distCoeffs = distortionCoefficients, 
        rvec = None, 
        tvec = None, 
        useExtrinsicGuess = False,
        flags = cv.SOLVEPNP_ITERATIVE
    )

    # Expected around 550 but actually -551.69314282
    print('tvec', tvec[2])

When using ppStrange the distance is totally flipped over, when using the rounded value ppNormal it's the expected (measured) somewhat around 550 mm. Switching to SOLVEPNP_EPNP is stabilizing the behavior, but seems to be less precise.

Any idea what causes the error and how to prevent or workaround this?

Tested with opencv 3.1.0.5 or 3.4.4. BTW.: Same behavior in c# with emgucv.

Edit: Overview:

With:

ppNormal = Vector2d(2591.661, 1728.591): 550.4662363

ppStrange = Vector2d(2591.66102211408, 1728.59081758821): -551.69314282