73 lines
1.8 KiB
Python
73 lines
1.8 KiB
Python
|
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\\"
|