I would like to find a fast implementation of distortion and undistortion of image points. Below are my current implementations which work well

# Given a set of distorted image points, I can use cv2.undistortpoints
def undistort_points(points, kmat, distcoeff):
    undistorted_pts_normed = cv2.undistortPoints(points, kmat, distcoeff).squeeze()
    cx = kmat[0,2]
    cy = kmat[1,2]
    fx = kmat[0,0]
    fy = kmat[1,1]
    undistorted_uv = np.empty(undistorted_pts_normed.shape)
    undistorted_uv[:,0] = fx * undistorted_pts_normed[:,0] + cx
    undistorted_uv[:,1] = fy * undistorted_pts_normed[:,1] + cy
    ret = [Point(x=int(pt[0]),y=int(pt[1])) for pt in undistorted_uv]

# To distort the points I use the distortion equations
def distort_points(points, kmat, distcoeff):
    cx = kmat[0,2]
    cy = kmat[1,2]
    fx = kmat[0,0]
    fy = kmat[1,1]
    k1,k2,p1,p2,k3 = distcoeff

    # Convert to normalized points
    normed_points = np.empty(points.shape)
    normed_points[:,0] = (points[:,0] - cx)/fx
    normed_points[:,1] = (points[:,1] - cy)/fy

    # Apply distortion
    r2 = normed_points[:,0]**2 + normed_points[:,1]**2 # r squared
    radial_dist = (1 + k1*r2 + k2*(r2**2) + k3*(r2**3))[...,np.newaxis]
    tangential_dist = np.empty(normed_points.shape)
    tangential_dist[:,0] = p2 * (r2 + 2 * (normed_points[:,0]**2)) + 2 * p1 * normed_points[:,0] * normed_points[:,1]
    tangential_dist[:,1] = p1 * (r2 + 2 * (normed_points[:,1]**2)) + 2 * p2 * normed_points[:,0] * normed_points[:,1]
    distorted_points = radial_dist * normed_points + tangential_dist

    # Return points in uv space
    distorted_uv = np.empty(distorted_points.shape)
    distorted_uv[:,0] = fx * distorted_points[:,0] + cx
    distorted_uv[:,1] = fy * distorted_points[:,1] + cy
    return distorted_uv

I’ve come across cv2.initUndistortRectifyMap which I can run once to store a mapping for each pixel to undistort, hence making the undistortion very efficient. However, I’m not sure how to use the mapx and mapy for image points instead of full images. Below shows how I compute the mapx and mapy mappings.

    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(kmat, distcoeff, (1920, 1080), 0, (1920, 1080))
    mapx, mapy = cv2.initUndistortRectifyMap(kmat, distcoeff, None, newcameramtx, (1920, 1080), 5)

I would also like to know if it’s possible to use the same mappings to convert from undistorted points to distorted points. Any help would be much appreciated!