C语言实现去畸变，不依赖opencv。

opencv 仅起读图和显示，去畸变部分是纯c。

#include <opencv2/opencv.hpp>
#include <iostream>
#include <cassert>
#include <cmath>
#include<iostream>
using namespace cv;
using namespace std;


int main()
{
    Mat image = cv::imread("test.png");
    int rows = image.rows, cols = image.cols;
    Mat image_undistort = Mat(rows, cols, CV_8UC3);
//begin
    double fx = 1761.11838;
    double fy = 1764.23318;
    double cx = 1252.36929;
    double cy = 726.936342;
    double k1 = -0.42143775;
    double k2 = 0.213623709;
    double p1 = 0.000174992651;
    double p2 = 0.0000262047557;
    double k3 =-0.0564630091;
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {
            double u_distorted = 0, v_distorted = 0;
            double x1, y1, x2, y2;
            x1 = (u - cx) / fx;
            y1 = (v - cy) / fy;
            double r2;
            r2 = pow(x1, 2) + pow(y1, 2);
            x2 = x1*(1 + k1*r2 + k2*pow(r2, 2)+ k3*pow(r2, 3)) + 2 * p1*x1*y1 + p2*(r2 + 2 * x1*x1);
            y2 = y1*(1 + k1*r2 + k2*pow(r2, 2)+ k3*pow(r2, 3)) + p1*(r2 + 2 * y1*y1) + 2 * p2*x1*y1;
            u_distorted = fx*x2 + cx;
            v_distorted = fy*y2 + cy;
//finish
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at<Vec3b>(v, u)[0] = image.at<Vec3b>((int)v_distorted, (int)u_distorted)[0];
                image_undistort.at<Vec3b>(v, u)[1] = image.at<Vec3b>((int)v_distorted, (int)u_distorted)[1];
                image_undistort.at<Vec3b>(v, u)[2] = image.at<Vec3b>((int)v_distorted, (int)u_distorted)[2];
            }
            else {
                image_undistort.at<Vec3b>(v, u) = {0,0,0};
            }
        }

    namedWindow("front",0);
    resizeWindow("front",1344,760);
    imshow("front", image);
    namedWindow("after",0);
    resizeWindow("after",1344,760);
    imshow("after",image_undistort);
    cv::waitKey(0);
    return 0;
}

附加一个缩放系数

double calc_inverse_matrix(float *matrix,char n,float *inverse_matrix)
{
    int i, j, k;
    float max, temp;
    float tmp[n][n];

    /* set matrix[n*n] to tmp[i][j] */
    for (i = 0; i < n; i++)
    {
        for (j = 0; j < n; j++)
        {
            tmp[i][j] = matrix[i * n + j];
        }
    }

    /* init inverse_matrix to be norm matrix */
    for (i = 0; i < n; i++)
    {
        for (j = 0; j < n; j++)
        {
            inverse_matrix[i *n + j] = (i == j) ? (float)1 : 0;
        }
    }

    for (i = 0; i < n; i++)
    {
        /* find matrix pivot element, find max bas(element) in matrix */
        max = tmp[i][i];
        k = i;
        for (j = i + 1; j < n; j++)
        {
            if (fabs(tmp[j][i]) > fabs(max))
            {
                max = tmp[j][i];
                k = j;
            }
        }

        /* if matrix pivot element not in first row, then change row[max] <=> row[0] */
        if (k != i)
        {
            for (j = 0; j < n; j++)
            {
                temp = tmp[i][j];
                tmp[i][j] = tmp[k][j];
                tmp[k][j] = temp;
                /* change inverse matrix*/
                temp = inverse_matrix[i *n + j];
                inverse_matrix[i *n + j] = inverse_matrix[k *n + j];
                inverse_matrix[k *n + j] = temp;
            }
        }

        /* if matrix pivot element == 0, There is no inverse matrix! */
        if (tmp[i][i] == 0)
        {
            printf("There is no inverse matrix!");
            exit(0);
        }

        temp = tmp[i][i];
        for (j = 0; j < n; j++)
        {
            tmp[i][j] = tmp[i][j] / temp;
            inverse_matrix[i *n + j] = inverse_matrix[i *n + j] / temp;
        }

        for (j = 0; j < n; j++)
        {
            if (j != i)
            {
                temp = tmp[j][i];
                for (k = 0; k < n; k++)
                {
                    tmp[j][k] = tmp[j][k] - tmp[i][k] * temp;
                    inverse_matrix[j * n + k] = inverse_matrix[j * n + k] - inverse_matrix[i *n + k] * temp;
                }
            }
        }
    }

    return 0;

}
/*  n = matrixA col = matrixB row */
double calc_matrix_multy(float *matrixA,float *matrixB,char n,float *matrixResult)
{
    for ( int i = 0; i < n; i++){
        for (int j = 0; j < n; j++){
            matrixResult[i * n + j] = 0;
            for (int k = 0; k < n; k++){
                matrixResult[i * n + j]+= matrixA[i * n + k] * matrixB[k * n + j];
            }
        }
    }
    return 0;
}


int main()
{
    Mat image = cv::imread("test.png");
    int rows = image.rows, cols = image.cols;
    Mat image_undistort = Mat(rows, cols, CV_8UC3);
    float fx = 1761.11838;
    float fy = 1764.23318;
    float cx = 1252.36929;
    float cy = 726.936342;
    float k1 = -0.42143775;
    float k2 = 0.213623709;
    float p1 = 0.000174992651;
    float p2 = 0.0000262047557;
    float k3 =-0.0564630091;
    float zoom_ratio = 0.5;
    float invsk[9],temp_array[9];
    float k[9] = {fx,0,cx,0,fy,cy,0,0,1};
    float sk[9]={zoom_ratio*fx,0,cx,0,zoom_ratio*fy,cy,0,0,1};
    calc_inverse_matrix(sk,3,invsk);
    calc_matrix_multy(k,invsk,3,temp_array);
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {
            double u_distorted = 0, v_distorted = 0;
            double x1, y1, x2, y2;
            float dst_x2 = temp_array[0] * u + temp_array[1]* v + temp_array[2];
            float dst_y2 = temp_array[3] * u + temp_array[4]* v + temp_array[5];
            x1 = (dst_x2 - cx) / fx;
            y1 = (dst_y2 - cy) / fy;
            double r2;
            r2 = pow(x1, 2) + pow(y1, 2);
            x2 = x1*(1 + k1*r2 + k2*pow(r2, 2)+ k3*pow(r2, 3)) + 2 * p1*x1*y1 + p2*(r2 + 2 * x1*x1);
            y2 = y1*(1 + k1*r2 + k2*pow(r2, 2)+ k3*pow(r2, 3)) + p1*(r2 + 2 * y1*y1) + 2 * p2*x1*y1;
            u_distorted = fx*x2 + cx;
            v_distorted = fy*y2 + cy;
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at<Vec3b>(v, u)[0] = image.at<Vec3b>((int)v_distorted, (int)u_distorted)[0];
                image_undistort.at<Vec3b>(v, u)[1] = image.at<Vec3b>((int)v_distorted, (int)u_distorted)[1];
                image_undistort.at<Vec3b>(v, u)[2] = image.at<Vec3b>((int)v_distorted, (int)u_distorted)[2];
            }
            else {
                image_undistort.at<Vec3b>(v, u) = {0,0,0};
            }
        }
    namedWindow("front",0);
    resizeWindow("front",1344,760);
    imshow("front", image);
    namedWindow("after",0);
    resizeWindow("after",1344,760);
    imshow("after",image_undistort);
    cv::waitKey(0);
    return 0;
}