Undistort single coordinates

edit

If you don't care about execution efficiency, you can use undistortPoints(). Here's an example: (original source)

std::vector<cv::Point2f> distorted_pts = ...             // your xys on image
std::vector<cv::Point2f> output_undistorted_points;

cv::Mat cam_k = ...              // distorted camera K matrix
cv::Mat cam_d = ...              // distorted camera distortion parameters
cv::Mat cam_k_rect =...          // Rectified camera matrix

cv::undistortPoints(distorted_pts,
                    output_undistorted_points,
                    cam_k, cam_d, 
                    cv::noArray(), 
                    cam_k_rect);

The rectified camera matrix can be acquired by getOptimalNewCameraMatrix() if you want further control over resulting image (i.e., crop to all valid pixels in rectified image or preserve all distorted image pixels).

Since the process requires iteration to find the coordinate in undistorted image given a distorted coordinate, the easiest way would be to build a map manually using undistortPoints():

std::Mat map(img.size(), CV_16SC2);
for( int col=0; col<img.width(); i++) {
    for( int row=0; row<img.height(); i++) {
        cv::Point2f px(col, row);
        cv::Point2f px_rect;
        cv::undistortPoints(px, px_rect, cam_k, cam_d, cv::noArray(), cam_k_rect);
        map.at<cv::Vec2s>(row, col)[0] = (short)px_rect.x;
        map.at<cv::Vec2s>(row, col)[1] = (short)px_rect.y;
    }
}

(Note that you can replace CV_16SC2 and Vec2d to CV_32FC2 and Vec2f for accurate storage)

Now you can find the distorted coordinate afterwards with

cv::Point2s px_tgt = ...;
cv::Point2s px_rect( map.at<cv::Vec2s>(px_tgt) );