广角摄像头畸变校正(2)-校正

//
// Created by root on 2020/10/31.
//
#include "opencv2/opencv.hpp"
#include <iostream>

using namespace cv;
using namespace std;

int main()
{
    String InputFile = "/home/cynthia/fov/lattice.jpeg";
    String OutputFile = "/home/cynthia/fov/lattice_校正.jpg";

    Mat sMatImg = imread(InputFile);
    if(sMatImg.empty())
    {
        printf("[LLE_PS_DEMO] error input is null!!\n");
        return -1;
    }
    Mat sMatOutput;
   
    double cameraMat[3][3] = {{848.5453557913092, 0, 1057.271793308861},
                               {0, 854.0626748731358, 533.0926382525498},
                               {0, 0, 1}};
    double distCoef[1][5] = {-0.242225843223194, 0.037612516019881, 0, 0, 0}
    
    Size image_size;
    image_size.width = 1920;
    image_size.height = 1080;

    Mat distCoefficients = Mat(1, 5, CV_64F, distCoef);
    Mat cameraMatrix = Mat(3,3,CV_64F,cameraMat);
    cout << "cameraMatrix" << cameraMatrix << endl;
    cout << "distCoefficients" << distCoefficients << endl;

    Mat mapx = Mat(image_size, CV_32FC1);
    Mat mapy = Mat(image_size, CV_32FC1);
    Mat R = Mat::eye(3, 3, CV_32F);
    //不优化cameraMatrix，默认使用原参数
    //initUndistortRectifyMap(cameraMatrix, distCoefficients, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
    //getOptimalNewCameraMatrix优化cameraMatrix，alpha=0不带黑边，alpha=1为带黑边全幅图像
    //C++: cv::getOptimalNewCameraMatrix=True设置固定主点不变
    initUndistortRectifyMap(cameraMatrix, distCoefficients, Mat(),
                            getOptimalNewCameraMatrix(cameraMatrix,distCoefficients, 
                            image_size, 0, image_size, 0,false),
                            image_size, CV_32FC1, mapx, mapy);

    Mat optimal_mat = getOptimalNewCameraMatrix(cameraMatrix, distCoefficients, image_size, 0, image_size, 0,false);
    cout << "optimal_mat" << optimal_mat << endl;

    resize(sMatImg, sMatOutput, image_size);
    Mat new_image = sMatOutput.clone();
    remap(sMatOutput, new_image, mapx, mapy, INTER_LINEAR);
    imwrite(OutputFile, new_image);
    
    return 0;
}

校正前

校正后

将参数写入.h文件

import os
import cv2
import numpy as np


def write_data_to_text_file(filename, list_data, data_num_per_line):
    if data_num_per_line <= 0 or data_num_per_line > len(list_data):
        data_num_per_line = 16

    f_output = open(filename, 'w+')
    f_output.write('const float previewMapY[] = \n')
    f_output.write('{\n')

    for i in range(0, len(list_data)):
        if (i != 0) and (i % data_num_per_line == 0):
            f_output.write('\n')
            f_output.write(str(list_data[i]) + ', ')
        elif (i + 1) == len(list_data):
            f_output.write(str(list_data[i]))
        else:
            f_output.write(str(list_data[i]) + ', ')
    f_output.write('\n};')
    f_output.close()


def rewrite_initUndistortRectifyMap(M, distCoeffs, R, newM, imageSize):
    image_w, image_h = imageSize
    imageSize = (image_h, image_w)
    R = np.asarray(R, np.float64)
    newM = np.asarray(newM[:, 0: 3], np.float64)  # 一些opencv和numpy不同的地方

    fx, fy, u0, v0 = M[0][0],  M[1][1],  M[0][2],  M[1][2]
    print(fx)
    print(fy)
    print(u0)
    print(v0)
    k1, k2, p1, p2, k3 = distCoeffs
    k4, k5, k6, s1, s2, s3, s4 = 0, 0, 0, 0, 0, 0, 0
    iR = np.linalg.inv(np.matmul(newM, R))
    print(iR)

    map_x = np.zeros(imageSize, dtype=np.float32)
    map_y = np.zeros(imageSize, dtype=np.float32)

    for i in range(image_h):
        for j in range(image_w):
            _x, _y, _w = np.matmul(iR, np.asarray([j, i, 1], dtype=np.float32))

            w = 1.0 / _w
            x, y = _x * w, _y * w
            x2, y2, _2xy = x * x, y * y, 2 * x * y
            r2 = x2 + y2
            kr = (1 + ((k3 * r2 + k2) * r2 + k1) * r2) / (1 + ((k6 * r2 + k5) * r2 + k4) * r2)
            xd = (x * kr + p1 * _2xy + p2 * (r2 + 2 * x2) + s1 * r2 + s2 * r2 * r2)
            yd = (y * kr + p1 * (r2 + 2 * y2) + p2 * _2xy + s3 * r2 + s4 * r2 * r2)

            map_x[i][j] = fx * xd + u0
            map_y[i][j] = fy * yd + v0
    return map_x, map_y

#16:9
# captureOriginM = np.array([[1904.36369837548, 0, 1930.81621262256],
#                            [0, 1905.6307172319, 1102.14708982119],
#                            [0, 0, 1]])
# previewOriginM = np.array([[1904.36369837548 / 3.0, 0, 1930.81621262256 / 3.0],
#                            [0, 1905.6307172319 / 3.0, 1102.14708982119 / 3.0],
#                            [0, 0, 1]])
# coeff = np.array([0.0525612447120403, -0.00540512035017591, 0, 0, -0.0497452534007338])
#
# captureM = np.array([[1981.158620923843, 0, 1920],
#                      [0, 1982.476733284634, 1080],
#                      [0, 0, 1]])
# previewM = np.array([[1981.158620923843 / 3.0, 0, 1920 / 3.0],
#                      [0, 1982.476733284634 / 3.0, 1080 / 3.0],
#                      [0, 0, 1]])


#4:3
captureOriginM = np.array([[1606.26308874934, 0, 1649.16046525146],
                           [0, 1605.82734578446, 1253.5433840553],
                           [0, 0, 1]])
previewOriginM = np.array([[1606.26308874934 / 3.4, 0, 1649.16046525146 / 3.4],
                           [0, 1605.82734578446 / 3.4, 1253.5433840553 / 3.4],
                           [0, 0, 1]])
coeff = np.array([0.09559582625022441, -0.09535455472047109, 0, 0, 0])

captureM = np.array([[1685.318074879634, 0, 1632],
                     [0, 1684.860886079208, 1224],
                     [0, 0, 1]])
previewM = np.array([[1685.318074879634 / 3.4, 0, 1632 / 3.4],
                     [0, 1684.860886079208 / 3.4,  1224 / 3.4],
                     [0, 0, 1]])

R = np.eye(3)

captureMapX, captureMapY = cv2.initUndistortRectifyMap(captureOriginM, coeff, R, captureM, (3264, 2448), cv2.CV_32FC1)
previewMapX, previewMapY = cv2.initUndistortRectifyMap(previewOriginM, coeff, R, previewM, (960, 720), cv2.CV_32FC1)

captureMapX = captureMapX.flatten()
captureMapY = captureMapY.flatten()
previewMapX = previewMapX.flatten()
previewMapY = previewMapY.flatten()
print(previewMapX.shape)
print(previewMapY.shape)
print(captureMapX.shape)
print(captureMapY.shape)

write_data_to_text_file("/home/ai/桌面/models/previewMapY4_3.h", previewMapY, 16)