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?

3

I suppose it comes from here: opencv/modules/calib3d/src/fisheye.cpp at 4.x · opencv/opencv · GitHub

I found an answer here: OpenCV Fisheye Calibration FOV 220. @crackwitz the answer is from you. :wink:

4

-1000000.0

so it’s the else-branch of that condition

either “theta flipped” or it’s some iteration with an epsilon criterium that failed to converge.

if you can, you should step into the library code and observe what’s going on in there.