鱼眼相机去畸变 Python/C++实现

陆辉东之前做了RealSense相机图像的远程传输，但是带畸变的图像

如果更进一步，可以一只fisheye带畸变，一只fisheye去畸变，这样放在QT界面里视觉感更好些

下午简单尝试了下，没有成功，还是要完成这项工作的

opencv 鱼眼矫正

【opencv】鱼眼图像畸变校正——透视变换

主要参照第一篇博客写了代码，但矫正后没什么效果

redwall@redwall-G3-3500:~$ rostopic list 
/camera/accel/imu_info
/camera/accel/metadata
/camera/accel/sample
/camera/fisheye1/camera_info
/camera/fisheye1/image_raw

redwall@redwall-G3-3500:~$ rostopic echo /camera/fisheye1/camera_info
header: 
  seq: 0
  stamp: 
    secs: 1662554565
    nsecs: 716393948
  frame_id: "camera_fisheye1_optical_frame"
height: 800
width: 848
distortion_model: "equidistant"
D: [-0.008326118811964989, 0.04620290920138359, -0.04403631016612053, 0.00837636087089777]
K: [286.1809997558594, 0.0, 421.6383972167969, 0.0, 286.3576965332031, 403.9013977050781, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [286.1809997558594, 0.0, 421.6383972167969, 0.0, 0.0, 286.3576965332031, 403.9013977050781, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

• 可以看到关于鱼眼相机的信息，其中包含distortion_model以及一些畸变矩阵，可能会用到
• do_rectify: False这里不知道是否和畸变校正相关

---

参考了下述GitHub包，已fork到个人仓库

https://github.com/HLearning/fisheye

主要学习了下面这段

def undistort(img_path,K,D,DIM,scale=0.6,imshow=False):
    img = cv2.imread(img_path)
    dim1 = img.shape[:2][::-1]  #dim1 is the dimension of input image to un-distort
    assert dim1[0]/dim1[1] == DIM[0]/DIM[1], "Image to undistort needs to have same aspect ratio as the ones used in calibration"
    if dim1[0]!=DIM[0]:
        img = cv2.resize(img,DIM,interpolation=cv2.INTER_AREA)
    Knew = K.copy()
    if scale:#change fov
        Knew[(0,1), (0,1)] = scale * Knew[(0,1), (0,1)]
    map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), Knew, DIM, cv2.CV_16SC2)
    undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
    if imshow:
        cv2.imshow("undistorted", undistorted_img)
    return undistorted_img

if __name__ == '__main__':
    '''
    DIM, K, D = get_K_and_D((6,9), '')
    '''
    DIM=(2560, 1920)
    K=np.array([[652.8609862494474, 0.0, 1262.1021584894233], [0.0, 653.1909758659955, 928.0871455436396], [0.0, 0.0, 1.0]])
    D=np.array([[-0.024092199861108887], [0.002745976275100771], [0.002545415522352827], [-0.0014366825722748522]])

    img = undistort('../imgs/pig.jpg',K,D,DIM)
    cv2.imwrite('../imgs/pig_checkerboard.jpg', img)

结合实验室相机参数做了一些简化，”/drag_video/scripts/fisheye_undistort.py”

#!/usr/bin/env python
import cv2
import numpy as np
import glob

def undistort(img_path,K,D,DIM,scale=1.0,imshow=False):
    img = cv2.imread(img_path)
    dim1 = img.shape[:2][::-1]  #dim1 is the dimension of input image to un-distort
    assert dim1[0]/dim1[1] == DIM[0]/DIM[1], "Image to undistort needs to have same aspect ratio as the ones used in calibration"
    if dim1[0]!=DIM[0]:
        img = cv2.resize(img,DIM,interpolation=cv2.INTER_AREA)
    Knew = K.copy()
    if scale:#change fov
        Knew[(0,1), (0,1)] = scale * Knew[(0,1), (0,1)]
    map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), Knew, DIM, cv2.CV_16SC2)
    undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
    if imshow:
        cv2.imshow("undistorted", undistorted_img)
    return undistorted_img

if __name__ == '__main__':

    DIM=(848, 800)
    K=np.array([[286.1809997558594, 0.0, 421.6383972167969,], [0.0, 286.3576965332031, 403.9013977050781], [0.0, 0.0, 1.0]])
    D=np.array([[-0.008326118811964989], [0.04620290920138359], [-0.04403631016612053], [0.00837636087089777]])

    img = undistort('../image/raw.jpg',K,D,DIM)
    cv2.imwrite('../image/processed.jpg', img)

校正效果还可以

校正前

校正后

但这是基于Python实现的，我Python不太行，使用时还参考了一些博客

ROS——创建工作空间和功能包并成功运行一个基本python文件

linux python错误解决：import: not authorized `xx’ @ error/constitute.c/WriteImage/1028.

在Linux中Python文件最前面需要加上下面这行才能正常运行

#!/usr/bin/env python

用C++重新实现了一下

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

using namespace std;
using namespace cv;

int main(int argc, char *argv[])
{
    Mat K = (cv::Mat_<double>(3, 3) << 286.1809997558594, 0.0, 421.6383972167969,
         0.0, 286.3576965332031, 403.9013977050781,
         0.0, 0.0, 1.0);
    Mat D = (cv::Mat_<double>(4, 1) << -0.008326118811964989, 0.04620290920138359, -0.04403631016612053, 0.00837636087089777);

    Mat raw_image = imread("/home/redwall/catkin_ws/src/push_video/image/raw.jpg");
    cout << raw_image.cols  << " " << raw_image.rows << endl;
    int width = 848;
    int height = 800;
    Mat map1, map2;
    Mat undistortImg;
    cv::Size imageSize(width, height);
    cv::fisheye::initUndistortRectifyMap(K, D, cv::Mat(), K, imageSize, CV_16SC2, map1, map2);
    // cout << "map1:" << map1 << "\nmap2:" << map2 << endl;
    remap(raw_image, undistortImg, map1, map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT);
    imwrite("/home/redwall/catkin_ws/src/push_video/image/cpp_processed.jpg", undistortImg);

    return 0;
}

主要就是两个函数

void cv::fisheye::initUndistortRectifyMap(cv::InputArray K, cv::InputArray D, cv::InputArray R, cv::InputArray P, const cv::Size &size, int m1type, cv::OutputArray map1, cv::OutputArray map2)
Computes undistortion and rectification maps for image transform by cv::remap(). If D is empty zero distortion is used, if R or P is empty identity matrixes are used.

参数:
K – Camera matrix
D – Input vector of distortion coefficients
R – Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1-channel or 1x1 3-channel
P – New camera matrix (3x3) or new projection matrix (3x4)
size – Undistorted image size.
m1type – Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps() for details.
map1 – The first output map.
map2 – The second output map.

void cv::remap(cv::InputArray src, cv::OutputArray dst, cv::InputArray map1, cv::InputArray map2, int interpolation, int borderMode = 0, const cv::Scalar &borderValue = cv::Scalar())
Applies a generic geometrical transformation to an image. The function remap transforms the source image using the specified map:

参数:
src – Source image.
dst – Destination image. It has the same size as map1 and the same type as src .
map1 – The first map of either (x,y) points or just x values having the type CV_16SC2 , CV_32FC1, or CV_32FC2. See convertMaps for details on converting a floating point representation to fixed-point for speed.
map2 – The second map of y values having the type CV_16UC1, CV_32FC1, or none (empty map if map1 is (x,y) points), respectively.
interpolation – Interpolation method (see cv::InterpolationFlags). The method INTER_AREA is not supported by this function.
borderMode – Pixel extrapolation method (see cv::BorderTypes). When borderMode=BORDER_TRANSPARENT, it means that the pixels in the destination image that corresponds to the "outliers" in the source image are not modified by the function.
borderValue – Value used in case of a constant border. By default, it is 0.

再看一眼相机的参数信息，在前面有记录

redwall@redwall-G3-3500:~$ rostopic echo /camera/fisheye1/camera_info
header: 
  seq: 0
  stamp: 
    secs: 1662554565
    nsecs: 716393948
  frame_id: "camera_fisheye1_optical_frame"
height: 800
width: 848
distortion_model: "equidistant"
D: [-0.008326118811964989, 0.04620290920138359, -0.04403631016612053, 0.00837636087089777]
K: [286.1809997558594, 0.0, 421.6383972167969, 0.0, 286.3576965332031, 403.9013977050781, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [286.1809997558594, 0.0, 421.6383972167969, 0.0, 0.0, 286.3576965332031, 403.9013977050781, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

注意到initUndistortRectifyMap()函数中if R or P is empty identity matrixes are used，因此参数cv::Mat()就是相机参数中的R阵，校正后效果如下图，和Python实现的基本一致

单张照片能够完成畸变校正，那么视频流也就很好实现了，将代码写成一个函数嵌入进现有推拉流框架即可

注意initUndistortRectifyMap() 只需计算一次即可，不需要每次循环都计算，因此可以将initUndistortRectifyMap() 放在循环外面

一开始设计拉流端鱼眼图像校正时，将initUndistortRectifyMap()函数放在循环中重复执行，因此造成了很大的延迟，将initUndistortRectifyMap()函数放到循环外仅执行一次后，延迟很小，可以忽略不计

同样，一开始设计推流端鱼眼图像校正时也翻了上述错误，虽然运行和推拉流延迟均较小，但还是浪费了计算资源，处理同上