import numpy as np
import os

GLOBAL_WIDTH = 9344
GLOBAL_HEIGH = 7000

# 相机内参矩阵，这里假设为简单的相机参数
global_w = GLOBAL_WIDTH
global_h = GLOBAL_HEIGH

MARK_COUNT = 4

# 相机内参
conf_fx = 1.15504192e+04
conf_fy = 1.15468773e+04
conf_cx = 4.67228001e+03
conf_cy = 3.49929103e+03

# 相机畸变参数
conf_k1 = -1.98130409e-02
conf_k2 = 4.46498961e-02
conf_p1 = 3.32909840e-04
conf_p2 = -4.24586368e-04
conf_k3 = 3.71045956e-01

# conf_fx = 11241.983
# conf_fy = 11244.0599
# conf_cx = 4553.03821
# conf_cy = 3516.9118

# conf_k1 = -0.04052072
# conf_k2 = 0.22211572
# conf_p1 = 0.00042405
# conf_p2 = -0.00367346
# conf_k3 = -0.15639485

# 相机内参矩阵
camera_matrix = np.array([
                    [conf_fx, 0, conf_cx],
                    [0, conf_fy, conf_cy],
                    [0,  0,  1]])

# 相机畸变矩阵
# dist_coeffs = np.array([k1, k2, p1, p2])  
dist_coeffs = np.array([conf_k1, conf_k2, conf_p1, conf_p2, conf_k3])  

T_vector = np.array([0.0, 0.0, 0.0])            # 初始化平移向量
# roll, pitch, yaw = 0.0, -1.5707963, 1.5707963   # 初始化旋转角 对应（x,y,z）
roll, pitch, yaw = 1.587794314984622, -1.5573540052770616, 0.01084704933694234 # 0.0034
roll, pitch, yaw = 1.5707963 , -1.5707963, 0.0  # 初始化旋转角 对应（x,y,z）
# roll,pitch,yaw = 1.5734601389822613, -1.556857147594011, 0.025063428135289487

# z 轴过滤
global_z_max = 3.5

# aruco 板尺寸定义
board_size = 0.5
marker_size = 0.35
b1 = 0.05
b2 = 0.05

# 雷达数据修正系数
kk = -0.01
kk = 0.0

# 2D投影区域选择系数
rmin = 3000000
rmax = 20000000

# 16点中最多过滤的点数
conf_max_exclude_num = 0

conf_temp_cloud_path = os.path.dirname(__file__) + "\\temp_cloud_data\\"