# Descirption:输入双目模组的相关参数 得到该参数下的模组的精度
# Author:xiaoxiong
# Dara:2022-03-06
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math


# 1280 800 104 76 60mm 1pixel maxdisparity
class stereoCameraModel(object):
    def __init__(self, iW, iH, hFOV, vFOV, baseline, maxdisparity, matchpercious):
        self.iW = iW  # 图像宽度
        self.iH = iH  # 图像高度
        self.horizontalFOV = hFOV*math.pi/180  # 相机水平方向FOV
        self.verticalFOV = vFOV*math.pi/180  # 相机竖直方向FOV
        self.focus = self.iW*0.5/math.tan(self.horizontalFOV/2)  # 相机焦距
        self.baseline = baseline  # 基线长度
        self.maxDisparity = maxdisparity # 算法最大视差
        self.matchPercious = matchpercious  # 左右目匹配精度
        self.blindMinDis = self.baseline * self.focus / self.maxDisparity  # 可测最小距离

    def print_cameraModelParameter(self):
        print('image width:{}; image height:{}'.format(self.iW, self.iH))
        print('camera horizontal FOV:{} degree; camera vertical FOV:{} degree'.format(self.horizontalFOV*180/math.pi, self.verticalFOV*180/math.pi))
        print('camera focus:{}; Module baseline:{} mm; Module Left Right match Percious:{} pixel'.format(self.focus, self.baseline, self.matchPercious))
        print('camera maxDisparity:{}; Module blindMinDis:{} mm'.format(self.maxDisparity, self.blindMinDis))


# 在实际匹配精度下模组在不同距离的精度
# z = f*b/d
    def getErrorInDistance(self, distance):
        d_current = self.focus*self.baseline/distance  # 当视差为d_current有distance
        error_Z = self.focus*self.baseline/math.fabs(d_current - self.matchPercious)
        delta_Z = math.fabs(error_Z-distance)
        Percious = (delta_Z/distance)*100
        return delta_Z, Percious


# 640宽 大FOV137 基线35mm 0.5个像素匹配精度
camera1 = stereoCameraModel(640, 480, 91, 77, 60, 192, 0.3)


print('camera1 #:')
camera1.print_cameraModelParameter()
for d in np.arange(100, 2200, 200):
    print('distance: {} mm; deltaZ: {} mm; Percious: {} %'.format(d, camera1.getErrorInDistance(d)[0], camera1.getErrorInDistance(d)[1]))