__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

    # 相機(jī)坐標(biāo)系到像素坐標(biāo)系的轉(zhuǎn)換矩陣
    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)


# 對(duì)攝像頭實(shí)時(shí)視頻流做畸變矯正
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()


# 對(duì)目錄下的所有圖片做畸變矯正，并把畸變矯正后的圖片保存下來(lái)
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()

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

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

    # 在這里把攝像頭的分辨率修改為和我們標(biāo)定時(shí)使用的一樣的分辨率 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()