【opencv】自己实现畸变校正

从高翔的《视觉SLAM十四讲》中记录的自己实现畸变校正

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "../distorted.png"; // 请确保路径正确

int main(int argc, char **argv)
{

    // 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file, 0); // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1); // 去畸变以后的图

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
    {
        for (int u = 0; u < cols; u++)
        {
            // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
            // 将三维空间点投影到归一化平面。设它的归一化坐标为[x,y]^T
            double x = (u - cx) / fx, y = (v - cy) / fy;
            double r = sqrt(x * x + y * y);
            // 对归一化平面上的点计算径向畸变和切向畸变
            double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
            double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
            // 将畸变后的点通过内参矩阵投影到像素平面，得到该点在图像上的正确位置
            double u_distorted = fx * x_distorted + cx;
            double v_distorted = fy * y_distorted + cy;

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows)
            {
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int)v_distorted, (int)u_distorted);
            }
            else
            {
                image_undistort.at<uchar>(v, u) = 0;
            }
        }
    }

    // 使用opencv自带的函数进行畸变校正
    cv::Mat cameraMatrix = (cv::Mat_<float>(3, 3) << 458.654, 0, 367.215,
                            0, 457.296, 248.375,
                            0, 0, 1);
    cv::Mat distCoeffs = (cv::Mat_<float>(1, 5) << -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0); // k1,k2,pi,p2,k3
    cv::Mat cvUndistortImage;
    cv::undistort(image, cvUndistortImage, cameraMatrix, distCoeffs);

    // 画图去畸变后图像
    cv::imshow("distorted", image);
    cout << "distorted.size() = " << image.size() << endl;
    cv::imshow("undistorted", image_undistort);
    cout << "undistorted.size() = " << image_undistort.size() << endl;
    cv::imshow("undistort use opencv", cvUndistortImage); //使用opencv自带的函数
    cout << "undistort use opencv size = " << cvUndistortImage.size() << endl;

    cv::waitKey();
    return 0;
}

程序地址：https://github.com/gaoxiang12/slambook2/blob/master/ch5/imageBasics/undistortImage.cpp
图片地址：https://github.com/gaoxiang12/slambook2/blob/master/ch5/imageBasics/distorted.png