【opencv】相机标定之畸变矫正与反畸变计算

原博客地址：https://www.cnblogs.com/mafuqiang/p/8134617.html

添加使用的例子：
myUndistortPointsFun.h

#if !defined(MYUNDISTORTPOINTSFUN)
#define MYUNDISTORTPOINTSFUN

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>

// 去畸变
void myUndistortPoints(const std::vector<cv::Point2d> &src, std::vector<cv::Point2d> &dst,
                       const cv::Mat &cameraMatrix, const cv::Mat &distortionCoeff);

// 使畸变
void myDistortPoints(const std::vector<cv::Point2d> &src, std::vector<cv::Point2d> &dst,
                     const cv::Mat &cameraMatrix, const cv::Mat &distortionCoeff);

#endif // MYUNDISTORTPOINTSFUN

myUndistortPointsFun.cpp

#include "myUndistortPointsFun.h"


//https://www.cnblogs.com/mafuqiang/p/8134617.html 

void myUndistortPoints(const std::vector<cv::Point2d> & src, std::vector<cv::Point2d> & dst,
    const cv::Mat & cameraMatrix, const cv::Mat & distortionCoeff)
{

    dst.clear();
    double fx = cameraMatrix.at<double>(0, 0);
    double fy = cameraMatrix.at<double>(1, 1);
    double ux = cameraMatrix.at<double>(0, 2);
    double uy = cameraMatrix.at<double>(1, 2);

    double k1 = distortionCoeff.at<double>(0, 0);
    double k2 = distortionCoeff.at<double>(0, 1);
    double p1 = distortionCoeff.at<double>(0, 2);
    double p2 = distortionCoeff.at<double>(0, 3);
    double k3 = distortionCoeff.at<double>(0, 4);
    double k4;
    double k5;
    double k6;

    for (unsigned int i = 0; i < src.size(); i++)
    {
        const cv::Point2d & p = src[i];
        // 首先进行坐标转换；
        double xDistortion = (p.x - ux) / fx;
        double yDistortion = (p.y - uy) / fy;

        double xCorrected, yCorrected;

        double x0 = xDistortion;
        double y0 = yDistortion;
        // 这里使用迭代的方式进行求解，因为根据2中的公式直接求解是困难的，所以通过设定初值进行迭代，这也是OpenCV的求解策略；
        for (int j = 0; j < 10; j++)
        {
            double r2 = xDistortion*xDistortion + yDistortion*yDistortion;

            double distRadialA = 1 / (1. + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
            double distRadialB = 1. + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2;

            double deltaX = 2. * p1 * xDistortion * yDistortion + p2 * (r2 + 2. * xDistortion * xDistortion);
            double deltaY = p1 * (r2 + 2. * yDistortion * yDistortion) + 2. * p2 * xDistortion * yDistortion;

            xCorrected = (x0 - deltaX)* distRadialA * distRadialB;
            yCorrected = (y0 - deltaY)* distRadialA * distRadialB;

            xDistortion = xCorrected;
            yDistortion = yCorrected;
        }
        // 进行坐标变换；
        xCorrected = xCorrected * fx + ux;
        yCorrected = yCorrected * fy + uy;

        dst.push_back(cv::Point2d(xCorrected, yCorrected));
    }

}

void myDistortPoints(const std::vector<cv::Point2d> & src, std::vector<cv::Point2d> & dst,
    const cv::Mat & cameraMatrix, const cv::Mat & distortionCoeff)
{

    dst.clear();
    double fx = cameraMatrix.at<double>(0, 0);
    double fy = cameraMatrix.at<double>(1, 1);
    double ux = cameraMatrix.at<double>(0, 2);
    double uy = cameraMatrix.at<double>(1, 2);

    double k1 = distortionCoeff.at<double>(0, 0);
    double k2 = distortionCoeff.at<double>(0, 1);
    double p1 = distortionCoeff.at<double>(0, 2);
    double p2 = distortionCoeff.at<double>(0, 3);
    double k3 = distortionCoeff.at<double>(0, 4);
    double k4;//
    double k5;//
    double k6;//

    for (unsigned int i = 0; i < src.size(); i++)
    {
        const cv::Point2d & p = src[i];
// 　　　　　//获取的点通常是图像的像素点，所以需要先通过小孔相机模型转换到归一化坐标系下；
        double xCorrected = (p.x - ux) / fx;
        double yCorrected = (p.y - uy) / fy;

        double xDistortion, yDistortion;
// 　　　　  //我们已知的是经过畸变矫正或理想点的坐标；
        double r2 = xCorrected*xCorrected + yCorrected*yCorrected;

        double deltaRa = 1. + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2;
        double deltaRb = 1 / (1. + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2);
        double deltaTx = 2. * p1 * xCorrected * yCorrected + p2 * (r2 + 2. * xCorrected * xCorrected);
        double deltaTy = p1 * (r2 + 2. * yCorrected * yCorrected) + 2. * p2 * xCorrected * yCorrected;
// 　　　　 //下面为畸变模型；
        xDistortion = xCorrected * deltaRa * deltaRb + deltaTx;
        yDistortion = yCorrected * deltaRa * deltaRb + deltaTy;
// 　　　　 //最后再次通过相机模型将归一化的坐标转换到像素坐标系下；
        xDistortion = xDistortion * fx + ux;
        yDistortion = yDistortion * fy + uy;

        dst.push_back(cv::Point2d(xDistortion, yDistortion));
    }

}

main.cpp

#include "myFunction/myUndistortPointsFun.h"

using namespace std;
using namespace cv;

int main(int argc, char *argv[])
{
    Mat background(Size(640, 640), CV_32FC3);
    Mat background2(Size(640, 640), CV_32FC3);
    Mat background3(Size(640, 640), CV_32FC3);

    imshow("background", background);
    vector<Point2d> line1;
    for (int j = 1; j < 9; j++)
    {
        for (int i = 10; i < 595; i++)
        {
            Point2d point(i, j * 50);
            line1.push_back(point);
        }
    }

    for (int i = 0; i < line1.size(); i++)
    {
        circle(background, line1[i], 1, Scalar(0, 0, 255), 1);
    }
    imshow("raw Points", background);

    // 相机内参
    Mat cameraMatrix = (Mat_<double>(3, 3) << 1229.210153358364, 0, 327.2633377834179,
                        0, 1230.528779625332, 214.8751296839213,
                        0, 0, 1);
    // 畸变矩阵
    Mat distCoeffs = (Mat_<double>(1, 5) << 0.8649656251092355, -34.57804001012766, 0.002187083265654973, 0.008358950285400273, 389.8609987216533);

    // 去畸变
    vector<Point2d> line2;
    myUndistortPoints(line1, line2, cameraMatrix, distCoeffs); // 使用去畸变校正函数
    for (int i = 0; i < line2.size(); i++)
    {
        circle(background2, line2[i], 1, Scalar(0, 0, 255), 1);
    }
    imshow("myUndistortPoints Points", background2);

    // 使畸变
    vector<Point2d> line3;
    myDistortPoints(line2, line3, cameraMatrix, distCoeffs); // 使畸变
    cout << line3.size() << endl;
    for (int i = 0; i < line3.size(); i++)
    {
        circle(background3, line3[i], 1, Scalar(0, 0, 255), 1);
    }
    imshow("mydistortPoints Points", background3);

    waitKey(0);
    return 0;
}