OpenCV: undistort (for images) and undistortPoints are inconsistent

Question

For testing I generate a grid image as matrix and again the grid points as point array:

This represents a "distorted" camera image along with some feature points. When I now undistort both the image and the grid points, I get the following result:

(Note that the fact that the "distorted" image is straight and the "undistorted" image is morphed is not the point, I'm just testing the undistortion functions with a straight test image.)

The grid image and the red grid points are totally misaligned now. I googled and found that some people forget to specify the "new camera matrix" parameter in undistortPoints but I didn't. The documentation also mentions a normalization but I still have the problem when I use the identity matrix as camera matrix. Also, in the central region it fits perfectly.

Why is this not identical, do I use something in a wrong way?

I use cv2 (4.1.0) in Python. Here is the code for testing:

import numpy as np
import matplotlib.pyplot as plt
import cv2

w = 401
h = 301


# helpers
#--------

def plotImageAndPoints(im, pu, pv):
    plt.imshow(im, cmap="gray")
    plt.scatter(pu, pv, c="red", s=16)
    plt.xlim(0, w)
    plt.ylim(0, h)
    plt.show()

def cv2_undistortPoints(uSrc, vSrc, cameraMatrix, distCoeffs):
    uvSrc = np.array([np.matrix([uSrc, vSrc]).transpose()], dtype="float32")
    uvDst = cv2.undistortPoints(uvSrc, cameraMatrix, distCoeffs, None, cameraMatrix)
    uDst = [uv[0] for uv in uvDst[0]]
    vDst = [uv[1] for uv in uvDst[0]]
    return uDst, vDst


# test data
#----------

# generate grid image
img = np.ones((h, w), dtype = "float32")
img[0::20, :] = 0
img[:, 0::20] = 0

# generate grid points
uPoints, vPoints = np.meshgrid(range(0, w, 20), range(0, h, 20), indexing='xy')
uPoints = uPoints.flatten()
vPoints = vPoints.flatten()

# see if points align with the image
plotImageAndPoints(img, uPoints, vPoints) # perfect!


# undistort both image and points individually
#---------------------------------------------

# camera matrix parameters
fx = 1
fy = 1
cx = w/2
cy = h/2

# distortion parameters
k1 = 0.00003
k2 = 0
p1 = 0
p2 = 0

# convert for opencv
mtx = np.matrix([
    [fx,  0, cx],
    [ 0, fy, cy],
    [ 0,  0,  1]
], dtype = "float32")

dist = np.array([k1, k2, p1, p2], dtype = "float32")

# undistort image
imgUndist = cv2.undistort(img, mtx, dist)
# undistort points
uPointsUndist, vPointsUndist = cv2_undistortPoints(uPoints, vPoints, mtx, dist)

# test if they still match
plotImageAndPoints(imgUndist, uPointsUndist, vPointsUndist) # awful!

Any help appreciated!

Answer 1

A bit late to the party, but to help others running into this issue: The problem is that UndistortPoints is an iterative calculation which in some cases exits before a stable solution has been reached. This can be fixed by modifying the termination criteria for the calculation, which can be done by using UndistortPointsIter. You should replace:

uvDst = cv2.undistortPoints(uvSrc, cameraMatrix, distCoeffs, None, cameraMatrix)

with:

uvDst = cv2.undistortPointsIter(uvSrc, cameraMatrix, distCoeffs, None, cameraMatrix,(cv2.TERM_CRITERIA_COUNT | cv2.TERM_CRITERIA_EPS, 40, 0.03))

Now, it tries 40 iterations to find a solution, rather than the default 5 iterations.