cv2.fisheye.undistortPoints with fisheye camera is not working

Python
,
1

I want to to estimate the motion of a monocular camera with a fisheye lens. The camera is a Raspberry Pi cam with 222° fisheye lens. The image resolution is 640x480.

I calibrated it using cv2.fisheye.calibrate(). I undistorted some images using
cv2.fisheye.undistortImage(image, K, D, Knew=K). The undistorted images look good.

I use this code to estimate R and t:

def lift_pixels_to_normalized(calib: datatypes.FisheyeCalib, pts_px: np.ndarray) → np.ndarray:
    """
    pts_px: Nx2 float32/float64 pixel coordinates.
    Returns Nx2 normalized coordinates on z=1 plane (i.e., rays ~ [x,y,1]).
    This does NOT undistort the image, only the point measurements.
    """
    pts = pts_px.reshape(-1, 1, 2).astype(np.float64)
    # P is identity => output in normalized camera coordinates (no K re-application).
    pts_norm = cv2.fisheye.undistortPoints(pts, calib.K, calib.D, P=np.eye(3))
    return pts_norm.reshape(-1, 2)

def estimate_motion_from_fisheye(calib: datatypes.FisheyeCalib, pts0_px: np.ndarray, pts1_px: np.ndarray):
    """
    Returns (R, t, inlier_mask) where R is 3x3, t is 3x1 (unknown scale), mask is Nx1 uint8.
    Uses normalized coordinates from fisheye undistortPoints.
    """
    pts0_n = lift_pixels_to_normalized(calib, pts0_px)
    pts1_n = lift_pixels_to_normalized(calib, pts1_px)

    # findEssentialMat expects points as Nx2; if focal=1, pp=(0,0), points must be normalized.
    # threshold is in normalized units (roughly "radians-ish" small angle). Tune if needed.
    E, mask = cv2.findEssentialMat(
        pts0_n, pts1_n,
        focal=1.0, pp=(0.0, 0.0),
        method=cv2.RANSAC,
        prob=0.999,
        threshold=1e-3
    )
    if E is None or mask is None:
        return None, None, None

    # recoverPose also accepts normalized points with focal=1, pp=(0,0)
    _, R, t, mask_pose = cv2.recoverPose(E, pts0_n, pts1_n, focal=1, pp=(0, 0), mask=mask)
    return R, t, mask_pose

The calibration matrix:

FisheyeCalib(
    K=array([[240.67924447,   0.        , 307.96438101],
             [  0.        , 240.58890106, 220.2128137 ],
             [  0.        ,   0.        ,   1.        ]]),
    D=array([[-0.02361354],
             [-0.02081853],
             [ 0.01779144],
             [-0.00738871]]))

Calling

lift_pixels_to_normalized(calib, np.array([[561.9933471679688, 30.114423751831055]]))

returns array([[-1000000., -1000000.]] which is totally wrong. This is not a valid point on the image plane.

2

and implausibly round. what’s going on with that?