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\\" |