# 测试双目摄像头消除畸变

import cv2
import numpy as np
from merge_img import merge_img

# 图像分辨率
imgHeight = 720
imgWidth = 1280

# 读取图像
imgLeft = cv2.imread('./BinaryTestImg/imgLeft_4.jpg')
imgRight = cv2.imread('./BinaryTestImg/imgRight_4.jpg')

# ========== Sec 00 : 消除畸变 ==========

# 内参矩阵 Matlab导出需要转置
leftIntrinsics = np.array([
    [1053.38265791018,  3.08399216907875,   594.257941839620],
    [0, 1048.69424857472,   624.403559476866],
    [0, 0,  1],
])

rightIntrinsics = np.array([
    [1031.31775398865,  0.995415539960191,  611.944908905225],
    [0, 1026.31331337872,   590.452376918678],
    [0, 0,  1],
])

# 畸变参数，{Radial[1, 2], Tangential[1, 2], Radial[3]}
leftDistortion = np.array(
    [-0.244068633392664,    0.0659908657440612, -0.0335847762686548,    -0.0105353212103192, 0]
)
rightDistortion = np.array(
    [-0.268439587426849,    0.0945428197668436, -0.0365036859972193,    -0.0108213400372969, 0]
)

# 关系向量
RotationVec = np.array([
    [0.999984802772915, -0.00130238961541062,   0.00535705184827462],
    [0.00132326239961219,   0.999991540412283,  -0.00389462199592219],
    [-0.00535193421458117,  0.00390165159375090,    0.999978066716967],
]) # Matlab导出，无需转置
TranslationVec = np.array(
    [-65.5771474903306, 1.82703288393659,   -0.224757807802837]
)

# 立体更正
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 =  cv2.stereoRectify(
        leftIntrinsics, leftDistortion, rightIntrinsics, rightDistortion,
        (imgHeight, imgWidth), RotationVec, TranslationVec)

# 计算更正map
leftMap1, leftMap2 = cv2.initUndistortRectifyMap(leftIntrinsics, leftDistortion, R1, P1, (imgWidth, imgHeight), cv2.CV_16SC2)
rightMap1, rightMap2 = cv2.initUndistortRectifyMap(rightIntrinsics, rightDistortion, R2, P2, (imgWidth, imgHeight), cv2.CV_16SC2)

imgLeftRemap = cv2.remap(imgLeft, leftMap1, leftMap2, cv2.INTER_LINEAR)
imgRightRemap = cv2.remap(imgRight, rightMap1, rightMap2, cv2.INTER_LINEAR)

cv2.imwrite('./BinaryTestImg/imgLeftRemap.jpg', imgLeftRemap)
cv2.imwrite('./BinaryTestImg/imgRightRemap.jpg', imgRightRemap)