fisheye::undistortPoints 源码

fisheye::undistortPoints 函数的源码如下：

void cv::fisheye::undistortPoints(InputArray distorted, OutputArray undistorted,
                                  InputArray K, InputArray D,
                                  InputArray R, InputArray P)
{
    CV_INSTRUMENT_REGION();

    CV_Assert(distorted.isMat() && distorted.depth() == CV_32F && distorted.channels() == 1);
    CV_Assert((K.empty() || K.type() == CV_64F) && (D.empty() || D.type() == CV_64F) &&
              (R.empty() || R.type() == CV_64F) && (P.empty() || P.type() == CV_64F));
    CV_Assert(!K.empty() && K.size() == Size(3, 3) && !D.empty() && D.total() >= 4 &&
              (R.empty() || R.size() == Size(3, 3)) && (P.empty() || P.size() == Size(3, 3)));

    Mat src = distorted.getMat(), intrinsics = K.getMat(), distCoeffs = D.getMat(),
        rotation = R.getMat(), newCamMat = P.getMat();

    int npoints = src.checkVector(2);
    CV_Assert(npoints >= 0);

    undistorted.create(npoints, 1, CV_32FC2);
    Mat dst = undistorted.getMat();

    Mat_<double> A, Ar, I = Mat_<double>::eye(3, 3);
    intrinsics.convertTo(A, CV_64F);
    if (!rotation.empty())
        rotation.convertTo(Ar, CV_64F);
    else
        A.copyTo(Ar);

    Mat_<double> Dk;
    distCoeffs.convertTo(Dk, CV_64F);

    Mat_<double> R_, P_;
    if (!rotation.empty())
        rotation.convertTo(R_, CV_64F);
    else
        R_ = I;

    if (!newCamMat.empty())
        newCamMat.convertTo(P_, CV_64F);
    else
    {
        P_.create(3, 3);
        A.copyTo(P_(Range(0, 3), Range(0, 3)));
        P_(0, 2) = src.cols * 0.5;
        P_(1, 2) = src.rows * 0.5;
    }

    Mat_<double> mx(1, npoints), my(1, npoints);
    double* _mx = mx[0], *_my = my[0];

    double fx = A(0, 0), fy = A(1, 1);
    double ifx = 1. / fx, ify = 1. / fy;
    double cx = A(0, 2), cy = A(1, 2);
    double k1 = Dk(0, 0), k2 = Dk(1, 0), k3 = Dk(4, 0), k4 = Dk(5, 0), k5 = Dk(6, 0), k6 = Dk(7, 0);

    const Point2f* srcf = src.ptr<Point2f>();
    for (int i = 0; i < npoints; i++)
    {
        double x = srcf[i].x, y = srcf[i].y;
        double x0 = x = (x - cx) * ifx;
        double y0 = y = (y - cy) * ify;
        double r2 = x * x + y * y;
        double theta = atan(sqrt(r2));
        double theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta4 * theta4;
        double theta_d = theta * (1 + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8);
        double inv_r = 1 / sqrt(r2);
        double cdist = x * x + y * y == 0 ? 1 : theta_d * inv_r;
        double x_d = x * cdist, y_d = y * cdist;
        if (dst.depth() == CV_32F)
        {
            _mx[i] = static_cast<float>(x_d * fx + cx);
            _my[i] = static_cast<float>(y_d * fy + cy);
        }
        else
        {
            _mx[i] = x_d * fx + cx;
            _my[i] = y_d * fy + cy;
        }
    }

    Mat mx_mat(1, npoints, CV_64F, mx.ptr<double>());
    Mat my_mat(1, npoints, CV_64F, my.ptr<double>());
    Mat XY;
    merge(mx_mat, my_mat, XY);
    if (!rotation.empty() || !newCamMat.empty())
        cv::transform(XY, XY, P_);
    else if (D.total() >= 5)
    {
        Mat_<double> scaled_XY(XY.size());
        cv::undistortPoints(XY, scaled_XY, A, Dk, Mat(), A);
        XY = scaled_XY;
    }

    Mat X = XY.col(0);
    Mat Y = XY.col(1);

    double* _X = X.ptr<double>();
    double* _Y = Y.ptr<double>();
    double* _dst = dst.ptr<double>();

    if (!rotation.empty())
    {
        Mat_<double> R = cv::Mat_<double>(3, 3, CV_64F, Ar.data());
        for (int i = 0; i < npoints; i++)
        {
            double x = _X[i], y = _Y[i], z = sqrt(max(1. - x * x - y * y, 0.));
            double Xr = _X[i], Yr = _Y[i], Zr = z;
            _X[i] = R(0, 0) * Xr + R(0, 1) * Yr + R(0, 2) * Zr;
            _Y[i] = R(1, 0) * Xr + R(1, 1) * Yr + R(1, 2) * Zr;
            double Z = R(2, 0) * Xr + R(2, 1) * Yr + R(2, 2) * Zr;
            _dst[i * 2] = _X[i] * Z;
            _dst[i * 2 + 1] = _Y[i] * Z;
        }
    }
    else
    {
        for (int i = 0; i < npoints; i++)
        {
            double x = _X[i], y = _Y[i], z = sqrt(max(1. - x * x - y * y, 0.));
            _dst[i * 2] = x * z;
            _dst[i * 2 + 1] = y * z;
        }
    }
}

这个函数的作用是将鱼眼相机图像中的畸变点进行矫正，输出矫正后的点坐标。函数的输入参数包括畸变点坐标、相机内参矩阵、畸变系数、旋转矩阵和新的相机内参矩阵。函数的输出参数是矫正后的点坐标。

函数的具体实现过程如下：

1. 通过输入参数获取畸变点坐标、相机内参矩阵、畸变系数、旋转矩阵和新的相机内参矩阵。
2. 对相机内参矩阵和旋转矩阵进行类型转换。
3. 根据相机内参矩阵和畸变系数计算矫正后的点坐标。
4. 如果有旋转矩阵或新的相机内参矩阵，则对矫正后的点坐标进行相应的变换。
5. 输出矫正后的点坐标。