import numpy as np
from scipy.optimize import minimize

#保存相机内参、外参等参数
# class Model:
#     def __init__(self):
#         # 内参
#         self.K = np.array([[801.8319, 0, 647.8920],
#                            [0, 801.7619, 532],
#                            [0, 0, 1]])
#         self.f = 3.6
#         self.H = 1019.0000170167332
#         self.dx = self.f / self.K[0, 0]
#         self.dy = self.f / self.K[1, 1]
#         self.u0 = self.K[0, 2]
#         self.v0 = self.K[1, 2]
#         # 外参
#         self.alpha = 0.7072338025822084
#         self.beta = 0.9077237961986776
#         # 位置修正
#         self.y = -70
#         self.x = 22
#         # 数据截断线
#         self.limit_slope = 0.3259949467095897
#         self.limit_intercept = 452.86565535382374
# self.K = np.array([[1086.58411235010, 0.00000000e+00, 656.379279306796],
#                     [0.00000000e+00, 1086.65442545872, 960-489.101890262552],
#                     [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
class Model:
    def __init__(self):
        # 内参
        self.K = np.array([[838.19297892,   0,         658.51580973],
                            [0,         836.93529706, 960-484.42225009],
                            [0,           0,           1]])
        # self.K = np.array([[1086.58411235010, 0.00000000e+00, 656.379279306796],
        #                     [0.00000000e+00, 1086.65442545872, 960-489.101890262552],
        #                     [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
        self.dist_coeffs = np.array([[-4.50688803e-01,  2.60760637e-01,  2.10619624e-04, - 6.69674309e-04, - 4.47846385e-02]])
        self.f = 3.6
        self.H = 1.007e+03
        self.dx = self.f / self.K[0, 0]
        self.dy = self.f / self.K[1, 1]
        self.u0 = self.K[0, 2]
        self.v0 = self.K[1, 2]
        # 外参
        self.alpha = 9.135e-01
        self.beta = 7.271e-01
        # 位置修正
        self.y = 0
        self.x = 0
        self.q = -2.892e-02