__Author__ = "Shliang"
__Email__ = "shliang0603@gmail.com"

import os
import cv2
import numpy as np
from tqdm import tqdm



def undistort(frame):
    fx = 685.646752
    cx = 649.107905
    fy = 676.658033
    cy = 338.054431
    k1, k2, p1, p2, k3 = -0.363219, 0.093818, 0.006178, -0.003714, 0.0

    # 相機坐標系到像素坐標系的轉換矩陣
    k = np.array([
        [fx, 0, cx],
        [0, fy, cy],
        [0, 0, 1]
    ])
    # 畸變系數(shù)
    d = np.array([
        k1, k2, p1, p2, k3
    ])
    h, w = frame.shape[:2]
    mapx, mapy = cv2.initUndistortRectifyMap(k, d, None, k, (w, h), 5)
    return cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)


# 對攝像頭實時視頻流做畸變矯正
def distortion_correction_cam():
    cap = cv2.VideoCapture(0)
    while (cap.isOpened()):
        ret, frame = cap.read()
        undistort_frame = undistort(frame)
        compare = np.hstack((frame, undistort_frame))
        cv2.imshow('frame', compare)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cap.release()
    cv2.destroyAllWindows()


# 對目錄下的所有圖片做畸變矯正，并把畸變矯正后的圖片保存下來
def distortion_correction_imgs(input_dir, output_dir):
    in_imgs = os.listdir(input_dir)

    for img_name in tqdm(in_imgs):
        image = cv2.imread(os.path.join(input_dir, img_name))
        distroted_img = undistort(image)
        cv2.imwrite(os.path.join(output_dir, img_name), distroted_img)



if __name__ == '__main__':
    input_dir = "/home/shl/extract_rosbag_data/0324_bags/plycal_calib/root/images"
    output_dir = "/home/shl/extract_rosbag_data/0324_bags/plycal_calib/root/distro_imgs"
    # distortion_correction_imgs(input_dir, output_dir)

    distortion_correction_cam()

對圖片進行矯正效果：

原圖：

矯正后的圖片：

采集的攝像頭畫面矯正效果：

從上面的換面可以看到，左邊是未矯正的畫面，右邊是矯正后的畫面：

矯正后的畫面會被裁切，明顯可以看到畫面中的信息是有裁切的，例如左邊的椅子已經(jīng)被裁切掉了
矯正后的畫面和原畫面是保持相同的分辨率：640x480，但是，為什么會看到畫面會出現(xiàn)橫向的拉伸，這是因為標定相機內參的時候畫面的分辨率設置為1280x720=16:9，但是opencv讀取攝像頭默認的分辨率卻是640x480=4:3，兩者的比例都不一樣，所以肯定會出現(xiàn)拉伸

解決拉伸的方式，就是把讀取攝像頭的時候，把攝像頭的分辨率設置成和標定的時候一樣的分辨率，設置為1280x720，下面是如何在opencv讀取攝像頭的時候設置攝像頭分辨率：

# 對攝像頭實時視頻流做畸變矯正
def distortion_correction_cam():
    cap = cv2.VideoCapture(0)

    # 獲取攝像頭讀取畫面的寬和高
    width = cap.get(3)
    height = cap.get(4)
    fps = cap.get(5)
    print(width, height, fps)  # 640.0 480.0 30.0

    # 在這里把攝像頭的分辨率修改為和我們標定時使用的一樣的分辨率 1280x720
    cap.set(3, 1280)
    cap.set(4, 720)
    width = cap.get(3)
    height = cap.get(4)
    print(width, height, fps)  # 1280.0 720.0 30.0


    while (cap.isOpened()):
        ret, frame = cap.read()
        print(frame.shape)
        undistort_frame = undistort(frame)
        compare = np.hstack((frame, undistort_frame))
        cv2.imshow('frame', compare)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cap.release()
    cv2.destroyAllWindows()

重新設置分辨率后，矯正前后畫面對比，可以看到幾乎是沒有橫向或縱向拉伸的！