Using EmguCV/OpenCV to correct optical distortions from photos

After calibrating a camera and obtaining the camera matrix and optical distortion coefficient matrix of the camera, the calculated matrices can be used to perform correction of photo images captured using the same camera. The open source libraries EmguCV and OpenCV have the methods and functions to undistort images given the parameters determined previously. The following snippets show how to perform the correction.

The following code C# snippet extends the EmguCV Mat class to enable us to get and set values in the matrix by row and column indices.

// Source: http://stackoverflow.com/questions/32255440/how-can-i-get-and-set-pixel-values-of-an-emgucv-mat-image

// Extends the EmguCV Mat class with Get and Set matrix values
    // by row and column indices
    public static class MatExtension
    {
        public static dynamic GetValue(this Mat mat, int row, int col)
        {
            var value = CreateElement(mat.Depth);
            Marshal.Copy(mat.DataPointer + (row * mat.Cols + col) * mat.ElementSize, value, 0, 1);
            return value[0];
        }

        public static void SetValue(this Mat mat, int row, int col, dynamic value)
        {
            var target = CreateElement(mat.Depth, value);
            Marshal.Copy(target, 0, mat.DataPointer + (row * mat.Cols + col) * mat.ElementSize, 1);
        }
        private static dynamic CreateElement(DepthType depthType, dynamic value)
        {
            var element = CreateElement(depthType);
            element[0] = value;
            return element;
        }

        private static dynamic CreateElement(DepthType depthType)
        {
            if (depthType == DepthType.Cv8S)
            {
                return new sbyte[1];
            }
            if (depthType == DepthType.Cv8U)
            {
                return new byte[1];
            }
            if (depthType == DepthType.Cv16S)
            {
                return new short[1];
            }
            if (depthType == DepthType.Cv16U)
            {
                return new ushort[1];
            }
            if (depthType == DepthType.Cv32S)
            {
                return new int[1];
            }
            if (depthType == DepthType.Cv32F)
            {
                return new float[1];
            }
            if (depthType == DepthType.Cv64F)
            {
                return new double[1];
            }
            return new float[1];
        }

The following code snippet is an example function that performs the correction to a list of JPEG images in a folder D:\Temp\.

public static void Undistort()
        {
            Mat cameraMatrix = new Mat(3, 3, DepthType.Cv64F, 1);
            Mat distCoeffs = new Mat(5, 1, DepthType.Cv64F, 1);
            Mat image = null;

            //Camera matrix values
            double fx = 3388.49;
            double fy = 3390.57;
            double cx = 2096.43;
            double cy = 1566.23;
            double skew = 1.11273;
            
            //Optical distortion coefficient values
            double k1 = 0.17352;
            double k2 = -0.484226;
            double k3 = 0.344761;
            double p1 = 0.00075256;
            double p2 = -0.000269617;

            //Set the camera matrix
            cameraMatrix.SetValue(0, 0, fx);
            cameraMatrix.SetValue(1, 1, fy);
            cameraMatrix.SetValue(0, 1, skew);
            cameraMatrix.SetValue(0, 2, cx);
            cameraMatrix.SetValue(1, 2, cy);
            cameraMatrix.SetValue(2, 2, 1);

            //Set the distortion matrix
            distCoeffs.SetValue(0, 0, k1);
            distCoeffs.SetValue(1, 0, k2);
            distCoeffs.SetValue(2, 0, p1);
            distCoeffs.SetValue(3, 0, p2);
            distCoeffs.SetValue(4, 0, k3);

            // get paths to image files
            string[] imageFiles = Directory.GetFiles(@"d:\temp\", "IMG_*.jpg");

            // for every image
            foreach (string imageFile in imageFiles)
            {
                // create new image object from file path.
                // append Und.jpg to the input filename for the output 
                string outFile = Path.Combine(Path.GetDirectoryName(imageFile), 
                    Path.GetFileNameWithoutExtension(imageFile) + "Und.jpg");

                Console.WriteLine("Processing {0}->{1}...", imageFile, outFile);
                
                //Read the input image
                image = CvInvoke.Imread(imageFile, LoadImageType.AnyColor);

                //Correct the input image
                Mat outFrame = image.Clone();
                CvInvoke.Undistort(image, outFrame, cameraMatrix, distCoeffs);
                
                //save the corrected image
                image = outFrame.Clone();
                image.Save(outFile);
                image.Dispose();
                outFrame.Dispose();
            }
        }