Fisheye Distortion Correction

Overview

This sample application performs a fisheye lens calibration using input images taken with the same camera/lens. Then it uses Remap and the calibration data to correct fisheye lens distortion of these images and save the result to disk. The mapping used for distortion correction is VPI_FISHEYE_EQUIDISTANT, which maps straight lines in the scene to straight lines in the corrected image.

Lens Calibration

Lens calibration uses a set of images taken by the same camera/lens, each one showing a checkerboard pattern in a different position, so that taken collectively, the checkerboard appears in almost entire field of view. The more images, the more accurate the calibration will be, but typically 10 to 15 images suffice.

VPI samples include a set of input images that can be used. They are found in /opt/nvidia/vpi3/samples/assets/fisheye directory.

Example of calibration images

To create a set of calibration images for a given lens, do the following:

1. Print a checkerboard pattern on a piece of paper. VPI provides in samples' assets directory one 10x7 checkerboard file that can be used, named checkerboard_10x7.pdf.
2. Mount the fisheye lens on a camera.
3. With the camera in a fixed position, take several pictures showing the checkerboard in different positions, covering a good part of the field of view.

Instructions

The command line parameters are:

-c W,H [-s win] <image1> [image2] [image3] ...

where

• -c W,H: specifies the number of squares the checkerboard pattern has horizontally (W) and vertically (H).
• -s win: (optional) the width of a window around each internal vertex of the checkerboard (point where 4 squares meet) to be used in a vertex position refinement stage. The actual vertex position will be searched within this window. If this parameter is omitted, the refinement stage will be skipped.
• imageN: set of calibration images

Here's one invocation example:

• C++

  ./vpi_sample_11_fisheye -c 10,7 -s 22 ../assets/fisheye/*.jpg

• Python

  python3 main.py -c 10,7 -s 22 ../assets/fisheye/*.jpg

This will correct the included set of calibration images, all captured using the checkerboard pattern also included. It's using a 22x22 window around each checkerboard internal vertex to refine the vertex position.

Results

Here are some input and output images produced by the sample application:

Input	Corrected

Source Code

For convenience, here's the code that is also installed in the samples directory.

Language: C++ Python

import sys
import vpi
import numpy as np
from argparse import ArgumentParser
import cv2
 
# ============================
# Parse command line arguments
 
parser = ArgumentParser()
parser.add_argument('-c', metavar='W,H', required=True,
                    help='Checkerboard with WxH squares')
 
parser.add_argument('-s', metavar='win', type=int,
                    help='Search window width around checkerboard verted used in refinement, default is 0 (disable refinement)')
 
parser.add_argument('images', nargs='+',
                    help='Input images taken with a fisheye lens camera')
 
args = parser.parse_args();
 
# Parse checkerboard size
try:
    cbSize = np.array([int(x) for x in args.c.split(',')])
except ValueError:
    exit("Error parsing checkerboard information")
 
# =========================================
# Calculate fisheye calibration from images
 
# OpenCV expects number of interior vertices in the checkerboard,
# not number of squares. Let's adjust for that.
vtxCount = cbSize-1
 
# -------------------------------------------------
# Determine checkerboard coordinates in image space
 
imgSize = None
corners2D = []
 
for imgName in args.images:
    # Load input image and do some sanity check
    img = cv2.imread(imgName)
    curImgSize = (img.shape[1], img.shape[0])
 
    if imgSize == None:
        imgSize = curImgSize
    elif imgSize != curImgSize:
        exit("All images must have the same size")
 
    # Find the checkerboard pattern on the image, saving the 2D
    # coordinates of checkerboard vertices in cbVertices.
    # Vertex is the point where 4 squares (2 white and 2 black) meet.
    found, corners = cv2.findChessboardCorners(img, tuple(vtxCount), flags=cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE)
    if found:
        # Needs to perform further corner refinement?
        if args.s != None and args.s >= 2:
            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.0001)
            imgGray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
            corners = cv2.cornerSubPix(imgGray, corners, (args.s//2, args.s//2), (-1,-1), criteria)
        corners2D.append(corners)
    else:
        exit("Warning: checkerboard pattern not found in image {}".format(input))
 
# Create the vector that stores 3D coordinates for each checkerboard pattern on a space
# where X and Y are orthogonal and run along the checkerboard sides, and Z==0 in all points on
# checkerboard.
cbCorners = np.zeros((1, vtxCount[0]*vtxCount[1], 3))
cbCorners[0,:,:2] = np.mgrid[0:vtxCount[0], 0:vtxCount[1]].T.reshape(-1,2)
corners3D = [cbCorners.reshape(-1,1,3) for i in range(len(corners2D))]
 
# ---------------------------------------------
# Calculate fisheye lens calibration parameters
camMatrix = np.eye(3)
coeffs = np.zeros((4,))
rms, camMatrix, coeffs, rvecs, tvecs = cv2.fisheye.calibrate(corners3D, corners2D, imgSize, camMatrix, coeffs, flags=cv2.fisheye.CALIB_FIX_SKEW)
 
# Print out calibration results
print("rms error: {}".format(rms))
print("Fisheye coefficients: {}".format(coeffs))
print("Camera matrix:")
print(camMatrix)
 
# ======================
# Undistort input images
 
# Create an uniform grid
grid = vpi.WarpGrid(imgSize)
 
# Create undistort warp map from the calibration parameters and the grid
undist_map = vpi.WarpMap.fisheye_correction(grid,
                                            K=camMatrix[0:2,:], X=np.eye(3,4), coeffs=coeffs,
                                            mapping=vpi.FisheyeMapping.EQUIDISTANT)
 
# Go through all input images,
idx=0
for imgName in args.images:
    # Load input image and do some sanity check
    img = cv2.imread(imgName)
 
    # Using the CUDA backend,
    with vpi.Backend.CUDA:
        # Convert image to NV12_ER, apply the undistortion map and convert image back to RGB8
        imgCorrected = vpi.asimage(img).convert(vpi.Format.NV12_ER).remap(undist_map, interp=vpi.Interp.CATMULL_ROM).convert(vpi.Format.RGB8)
 
    # Write undistorted image to disk
    cv2.imwrite("undistort_python{}_{:03d}.jpg".format(sys.version_info[0],idx), imgCorrected.cpu())
    idx += 1

#include <opencv2/core/version.hpp>
 
#if CV_MAJOR_VERSION >= 3
#    include <opencv2/imgcodecs.hpp>
#else
#    include <opencv2/highgui/highgui.hpp>
#endif
 
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vpi/OpenCVInterop.hpp>
 
#include <vpi/Image.h>
#include <vpi/LensDistortionModels.h>
#include <vpi/Status.h>
#include <vpi/Stream.h>
#include <vpi/algo/ConvertImageFormat.h>
#include <vpi/algo/Remap.h>
 
#include <iostream>
#include <sstream>
 
#define CHECK_STATUS(STMT)                                    \
    do                                                        \
    {                                                         \
        VPIStatus status = (STMT);                            \
        if (status != VPI_SUCCESS)                            \
        {                                                     \
            char buffer[VPI_MAX_STATUS_MESSAGE_LENGTH];       \
            vpiGetLastStatusMessage(buffer, sizeof(buffer));  \
            std::ostringstream ss;                            \
            ss << vpiStatusGetName(status) << ": " << buffer; \
            throw std::runtime_error(ss.str());               \
        }                                                     \
    } while (0);
 
static void PrintUsage(const char *progname, std::ostream &out)
{
    out << "Usage: " << progname << " <-c W,H> [-s win] <image1> [image2] [image3] ...\n"
        << " where,\n"
        << " W,H\tcheckerboard with WxH squares\n"
        << " win\tsearch window width around checkerboard vertex used\n"
        << "\tin refinement, default is 0 (disable refinement)\n"
        << " imageN\tinput images taken with a fisheye lens camera" << std::endl;
}
 
static char *my_basename(char *path)
{
#ifdef WIN32
    char *name = strrchr(path, '\\');
#else
    char *name = strrchr(path, '/');
#endif
    if (name != NULL)
    {
        return name;
    }
    else
    {
        return path;
    }
}
 
struct Params
{
    cv::Size vtxCount;                // Number of internal vertices the checkerboard has
    int searchWinSize;                // search window size around the checkerboard vertex for refinement.
    std::vector<const char *> images; // input image names.
};
 
static Params ParseParameters(int argc, char *argv[])
{
    Params params = {};
 
    cv::Size cbSize;
 
    for (int i = 1; i < argc; ++i)
    {
        if (argv[i][0] == '-')
        {
            if (strlen(argv[i] + 1) == 1)
            {
                switch (argv[i][1])
                {
                case 'h':
                    PrintUsage(my_basename(argv[0]), std::cout);
                    return {};
 
                case 'c':
                    if (i == argc - 1)
                    {
                        throw std::invalid_argument("Option -c must be followed by checkerboard width and height");
                    }
 
                    if (sscanf(argv[++i], "%d,%d", &cbSize.width, &cbSize.height) != 2)
                    {
                        throw std::invalid_argument("Error parsing checkerboard information");
                    }
 
                    // OpenCV expects number of interior vertices in the checkerboard,
                    // not number of squares. Let's adjust for that.
                    params.vtxCount.width  = cbSize.width - 1;
                    params.vtxCount.height = cbSize.height - 1;
                    break;
 
                case 's':
                    if (i == argc - 1)
                    {
                        throw std::invalid_argument("Option -s must be followed by search window size");
                    }
                    if (sscanf(argv[++i], "%d", &params.searchWinSize) != 1)
                    {
                        throw std::invalid_argument("Error parsing search window size");
                    }
                    if (params.searchWinSize < 0)
                    {
                        throw std::invalid_argument("Search window size must be >= 0");
                    }
                    break;
 
                default:
                    throw std::invalid_argument(std::string("Option -") + (argv[i] + 1) + " not recognized");
                }
            }
            else
            {
                throw std::invalid_argument(std::string("Option -") + (argv[i] + 1) + " not recognized");
            }
        }
        else
        {
            params.images.push_back(argv[i]);
        }
    }
 
    if (params.images.empty())
    {
        throw std::invalid_argument("At least one image must be defined");
    }
 
    if (cbSize.width <= 3 || cbSize.height <= 3)
    {
        throw std::invalid_argument("Checkerboard size must have at least 3x3 squares");
    }
 
    if (params.searchWinSize == 1)
    {
        throw std::invalid_argument("Search window size must be 0 (default) or >= 2");
    }
 
    return params;
}
 
int main(int argc, char *argv[])
{
    // OpenCV image that will be wrapped by a VPIImage.
    // Define it here so that it's destroyed *after* wrapper is destroyed
    cv::Mat cvImage;
 
    // VPI objects that will be used
    VPIStream stream = NULL;
    VPIPayload remap = NULL;
    VPIImage tmpIn = NULL, tmpOut = NULL;
    VPIImage vimg = nullptr;
 
    int retval = 0;
 
    try
    {
        // First parse command line paramers
        Params params = ParseParameters(argc, argv);
        if (params.images.empty()) // user just wanted the help message?
        {
            return 0;
        }
 
        // Where to store checkerboard 2D corners of each input image.
        std::vector<std::vector<cv::Point2f>> corners2D;
 
        // Store image size. All input images must have same size.
        cv::Size imgSize = {};
 
        for (unsigned i = 0; i < params.images.size(); ++i)
        {
            // Load input image and do some sanity check
            cv::Mat img = cv::imread(params.images[i]);
            if (img.empty())
            {
                throw std::runtime_error("Can't read " + std::string(params.images[i]));
            }
 
            if (imgSize == cv::Size{})
            {
                imgSize = img.size();
            }
            else if (imgSize != img.size())
            {
                throw std::runtime_error("All images must have same size");
            }
 
            // Find the checkerboard pattern on the image, saving the 2D
            // coordinates of checkerboard vertices in cbVertices.
            // Vertex is the point where 4 squares (2 white and 2 black) meet.
            std::vector<cv::Point2f> cbVertices;
 
            if (findChessboardCorners(img, params.vtxCount, cbVertices,
                                      cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE))
            {
                // Needs to perform further corner refinement?
                if (params.searchWinSize >= 2)
                {
                    cv::Mat gray;
                    cvtColor(img, gray, cv::COLOR_BGR2GRAY);
 
                    cornerSubPix(gray, cbVertices, cv::Size(params.searchWinSize / 2, params.searchWinSize / 2),
                                 cv::Size(-1, -1),
                                 cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.0001));
                }
 
                // save this image's 2D vertices in vector
                corners2D.push_back(std::move(cbVertices));
            }
            else
            {
                std::cerr << "Warning: checkerboard pattern not found in image " << params.images[i] << std::endl;
            }
        }
 
        // Create the vector that stores 3D coordinates for each checkerboard pattern on a space
        // where X and Y are orthogonal and run along the checkerboard sides, and Z==0 in all points on
        // checkerboard.
        std::vector<cv::Point3f> initialCheckerboard3DVertices;
        for (int i = 0; i < params.vtxCount.height; ++i)
        {
            for (int j = 0; j < params.vtxCount.width; ++j)
            {
                // since we're not interested in extrinsic camera parameters,
                // we can assume that checkerboard square size is 1x1.
                initialCheckerboard3DVertices.emplace_back(static_cast<float>(j), static_cast<float>(i), 0.0f);
            }
        }
 
        // Initialize a vector with initial checkerboard positions for all images
        std::vector<std::vector<cv::Point3f>> corners3D(corners2D.size(), initialCheckerboard3DVertices);
 
        // Camera intrinsic parameters, initially identity (will be estimated by calibration process).
        using Mat3     = cv::Matx<double, 3, 3>;
        Mat3 camMatrix = Mat3::eye();
 
        // stores the fisheye model coefficients.
        std::vector<double> coeffs(4);
 
        // VPI currently doesn't support skew parameter on camera matrix, make sure
        // calibration process fixes it to 0.
        int flags = cv::fisheye::CALIB_FIX_SKEW;
 
        // Run calibration
        {
            cv::Mat rvecs, tvecs; // stores rotation and translation for each camera, not needed now.
            double rms = cv::fisheye::calibrate(corners3D, corners2D, imgSize, camMatrix, coeffs, rvecs, tvecs, flags);
            printf("rms error: %lf\n", rms);
        }
 
        // Output calibration result.
        printf("Fisheye coefficients: %lf %lf %lf %lf\n", coeffs[0], coeffs[1], coeffs[2], coeffs[3]);
 
        printf("Camera matrix:\n");
        printf("[%lf %lf %lf; %lf %lf %lf; %lf %lf %lf]\n", camMatrix(0, 0), camMatrix(0, 1), camMatrix(0, 2),
               camMatrix(1, 0), camMatrix(1, 1), camMatrix(1, 2), camMatrix(2, 0), camMatrix(2, 1), camMatrix(2, 2));
 
        // Now use VPI to undistort the input images:
 
        // Allocate a dense map.
        VPIWarpMap map            = {};
        map.grid.numHorizRegions  = 1;
        map.grid.numVertRegions   = 1;
        map.grid.regionWidth[0]   = imgSize.width;
        map.grid.regionHeight[0]  = imgSize.height;
        map.grid.horizInterval[0] = 1;
        map.grid.vertInterval[0]  = 1;
        CHECK_STATUS(vpiWarpMapAllocData(&map));
 
        // Initialize the fisheye lens model with the coefficients given by calibration procedure.
        VPIFisheyeLensDistortionModel distModel = {};
        distModel.mapping                       = VPI_FISHEYE_EQUIDISTANT;
        distModel.k1                            = coeffs[0];
        distModel.k2                            = coeffs[1];
        distModel.k3                            = coeffs[2];
        distModel.k4                            = coeffs[3];
 
        // Fill up the camera intrinsic parameters given by camera calibration procedure.
        VPICameraIntrinsic K;
        for (int i = 0; i < 2; ++i)
        {
            for (int j = 0; j < 3; ++j)
            {
                K[i][j] = camMatrix(i, j);
            }
        }
 
        // Camera extrinsics is be identity.
        VPICameraExtrinsic X = {};
        X[0][0] = X[1][1] = X[2][2] = 1;
 
        // Generate a warp map to undistort an image taken from fisheye lens with
        // given parameters calculated above.
        vpiWarpMapGenerateFromFisheyeLensDistortionModel(K, X, K, &distModel, &map);
 
        // Create the Remap payload for undistortion given the map generated above.
        CHECK_STATUS(vpiCreateRemap(VPI_BACKEND_CUDA, &map, &remap));
 
        // Now that the remap payload is created, we can destroy the warp map.
        vpiWarpMapFreeData(&map);
 
        // Create a stream where operations will take place. We're using CUDA
        // processing.
        CHECK_STATUS(vpiStreamCreate(VPI_BACKEND_CUDA, &stream));
 
        // Temporary input and output images in NV12 format.
        CHECK_STATUS(vpiImageCreate(imgSize.width, imgSize.height, VPI_IMAGE_FORMAT_NV12_ER, 0, &tmpIn));
        CHECK_STATUS(vpiImageCreate(imgSize.width, imgSize.height, VPI_IMAGE_FORMAT_NV12_ER, 0, &tmpOut));
 
        // For each input image,
        for (unsigned i = 0; i < params.images.size(); ++i)
        {
            // Read it from disk.
            cvImage = cv::imread(params.images[i]);
            assert(!cvImage.empty());
 
            // Wrap it into a VPIImage
            if (vimg == nullptr)
            {
                // Now create a VPIImage that wraps it.
                CHECK_STATUS(vpiImageCreateWrapperOpenCVMat(cvImage, 0, &vimg));
            }
            else
            {
                CHECK_STATUS(vpiImageSetWrappedOpenCVMat(vimg, cvImage));
            }
 
            // Convert BGR -> NV12
            CHECK_STATUS(vpiSubmitConvertImageFormat(stream, VPI_BACKEND_CUDA, vimg, tmpIn, NULL));
 
            // Undistorts the input image.
            CHECK_STATUS(vpiSubmitRemap(stream, VPI_BACKEND_CUDA, remap, tmpIn, tmpOut, VPI_INTERP_CATMULL_ROM,
                                        VPI_BORDER_ZERO, 0));
 
            // Convert the result NV12 back to BGR, writing back to the input image.
            CHECK_STATUS(vpiSubmitConvertImageFormat(stream, VPI_BACKEND_CUDA, tmpOut, vimg, NULL));
 
            // Wait until conversion finishes.
            CHECK_STATUS(vpiStreamSync(stream));
 
            // Since vimg is wrapping the OpenCV image, the result is already there.
            // We just have to save it to disk.
            char buf[64];
            snprintf(buf, sizeof(buf), "undistort_%03d.jpg", i);
            imwrite(buf, cvImage);
        }
    }
    catch (std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        PrintUsage(my_basename(argv[0]), std::cerr);
 
        retval = 1;
    }
 
    vpiStreamDestroy(stream);
    vpiPayloadDestroy(remap);
    vpiImageDestroy(tmpIn);
    vpiImageDestroy(tmpOut);
    vpiImageDestroy(vimg);
 
    return retval;
}