[SOLVED] How do I use cv2.fisheye.undistortPoints to convert a point in distorted space to one in undistorted space?

How do I use cv2.fisheye.undistortPoints to convert a point in distorted space to one in undistorted space?

I am trying to map features from undistorted space back to distorted space with the opencv fisheye functions. I can successfully transform my image from the camera distorted fisheye image to regular space via this code:

DIM = (953, 720)
K = np.array(
    [
        [407.0259762535615, 0.0, 488.89663712932474],
        [0.0, 409.25366832487896, 388.1998354574297],
        [0.0, 0.0, 1.0],
    ]
)
D = np.array(
    [
        [-0.04485892302426824],
        [0.0787884305594057],
        [-0.08374236678783106],
        [0.027626067632899026],
    ]
)
img = np.zeros((720, 953, 3), dtype=np.uint8)
img = cv2.rectangle(img, (200, 150), (300, 200), (255, 255, 255), -1)
map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2)
undistorted_img = cv2.remap(
    img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT
)
# The rectangle is now found at 60, 43 in undistorted_img.  This code works.

However, when I want to map a point (either direction), I am unable to use cv2.fisheye.undistortPoints or distortPoints to move a point from one space to another. By using an image with dimensions 720 x 953, I put a point at x,y 200,150. In the undistorted image, the point is now at 60,43. However, I can't map these two points using either function. Here's my code and the outputs:

cv2.fisheye.undistortPoints(np.array([[[200, 150]]], dtype=np.float32), K, D)
# returns array([[[-1.0488918, -0.8601203]]], dtype=float32)
cv2.fisheye.distortPoints(np.array([[[200, 150]]], dtype=np.float32), K, D)
# returns array([[[1064.9419,  822.5983]]], dtype=float32)
cv2.fisheye.undistortPoints(np.array([[[60, 34]]], dtype=np.float32), K, D)
# Returns array([[[-4.061374 , -3.3357866]]], dtype=float32)
cv2.fisheye.distortPoints(np.array([[[60, 34]]], dtype=np.float32), K, D)
# array([[[1103.0706 ,  738.13654]]], dtype=float32)

None of this matches what I see in the image transformation itself. What am I not understanding about distortPoints and undistortPoints? Thanks!

Solution

I think you need to estimate new camera matrix for undistortion.

points = np.array([[[200, 150]]]).astype(np.float32)
newcameramtx = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(
    K, D, DIM, None, balance=1)
dst = cv2.fisheye.undistortPoints(points, K, D, None, newcameramtx)
# [[[323.35104 242.06458]]]