-
Notifications
You must be signed in to change notification settings - Fork 1
/
orbbec_mag_joint_calibration.py
164 lines (152 loc) · 8.52 KB
/
orbbec_mag_joint_calibration.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
import os
import sys
PROJECT_ABSOLUTE_PATH = os.path.dirname(os.path.abspath(__file__))
sys.path.append(PROJECT_ABSOLUTE_PATH)
import argparse
import glob
import cv2
import numpy as np
from utils.calibrate import get_obj_points, cal_internal_monocular, cal_outside_image_monocular, find_chessboard_corners
from orbbec_camera.orbbec_astra_Sample import load_data, get_world_coordinate
from orbbec_mag_coordinates_transform import load_joint_parameter
def cal_world_coordinates(img_list, checker_board, square_size, img_dir_lst, lib_path, camera_type, obj_orbbec=None, corner_success=[], tran_matrix=None):
"""
计算内角点真实世界坐标，调用奥比中光sdk获得真实世界坐标，获得的世界坐标是基于奥比中光建立的世界坐标系
如果深度相机没有对应sdk，应根据深度文件加外部测量人工测量世界坐标，单位一般是毫米
"""
import copy
# 存储3D点
obj_points = []
if camera_type == 'orbbec':
corner_success = []
cnt_mag = 0
# 遍历图片列表
for idx, img_ in enumerate(img_list):
# 将图片转为灰度图
gray = cv2.cvtColor(img_, cv2.COLOR_BGR2GRAY)
# 寻找棋盘格上的亚像素角点
ret, corners = find_chessboard_corners(gray, checker_board)
depth_name = img_dir_lst[idx].replace("rgb.jpg", "depth.pkl")
# 寻找成功
if ret:
if camera_type == 'orbbec':
corners_int = np.around(copy.deepcopy(corners).reshape(1, checker_board[0] * checker_board[1], 2), 0).astype(np.int64)
depth_stream, depth_data = load_data("", depth_name, lib_path)
obj_p = get_obj_points(checker_board, square_size)
flag = False
for i, coordinate in enumerate(corners_int[0]):
if tran_matrix is not None:
coordinate = np.hstack((coordinate, 1)).reshape(1, 3)
coordinate = np.squeeze(np.around(coordinate.dot(tran_matrix.T), 0).astype(np.int64))
x_w, y_w, z_w = get_world_coordinate(depth_stream, depth_data, coordinate)
if x_w is None:
flag = True
break
obj_p[0, i, 0] = x_w
obj_p[0, i, 1] = y_w
obj_p[0, i, 2] = z_w
# if (i + 1) % 6 == 0:
# for j in range(i - 5, i):
# print(np.linalg.norm(obj_p[0, j, :] - obj_p[0, j + 1, :]))
# print(obj_p[0, i, :], i)
if flag:
corner_success.append(False)
continue
corner_success.append(True)
# 将世界坐标添加到世界坐标列表中
obj_points.append(obj_p)
else:
if corner_success[idx]:
# 将世界坐标添加到世界坐标列表中
obj_points.append(obj_orbbec[cnt_mag])
cnt_mag += 1
else:
if camera_type == 'orbbec':
corner_success.append(False)
else:
if corner_success[idx]:
corner_success[idx] = False
cnt_mag += 1
return obj_points, corner_success
def joint_calibration():
"""
寻找标定板交点，在此基础上寻找亚像素焦点优化。
:return: 奥比中光: 内参矩阵K1，畸变系数D1，旋转向量rvec1，旋转矩阵R1，平移向量T1 巨哥科技: 内参矩阵K2，畸变系数D2，旋转向量rvec2，旋转矩阵R2，平移向量T2
"""
def parse_args():
"""
:param checker_board: 棋盘格内角点数，格式为元组
:param square_size: 棋盘方格真实长宽，格式为元组，单位毫米
:param sample_path: 采集数据保存地址，采集数据可用sample/joint_calibration_sample.py脚本，
尽量选择其中角点检测成功且清晰的数据提高标定质量（不筛选也行，但可能影响结果精度），
名字带rendered画出了角点，红色是检测的，绿色是根据默认参数匹配的，标定后参数会自动更新
:param lib_path: 奥比中光openni sdk路径,路径到/sdk/libs
:param tran_matrix: 奥比中光rgb对齐到深度图像的变换矩阵文件路径
"""
parser = argparse.ArgumentParser()
parser.add_argument('--checker_board', nargs='+', type=int, help='checker board size')
parser.add_argument('--square_size', nargs='+', type=int, help='checker board square real size')
parser.add_argument('--lib_path', type=str, help='orbbec sdk lib path')
parser.add_argument('--sample_path', type=str, default=os.path.join(PROJECT_ABSOLUTE_PATH, "sample"), help='sampling data saved dir')
parser.add_argument('--tran_matrix', type=str, default=os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/tran_matrix.txt"), help='transform matrix saved dir')
return parser.parse_args()
args = parse_args()
checker_board = tuple(args.checker_board)
square_size = tuple(args.square_size)
sample_path = args.sample_path
lib_path = args.lib_path
tran_matrix = np.loadtxt(args.tran_matrix)
orbbec_img_dir_lst = glob.glob(os.path.join(sample_path, "*orbbec_rgb.jpg"))
mag_img_dir_lst = glob.glob(os.path.join(sample_path, "*MAG_rgb.jpg"))
# calibrate orbbec
orbbec_img_list = []
for img_dir in orbbec_img_dir_lst:
orbbec_img_list.append(cv2.imread(img_dir))
obj_p = get_obj_points(checker_board, square_size)
ret, K1, D1 = cal_internal_monocular(obj_p, orbbec_img_list, checker_board)
# calibrate magnity
mag_img_list = []
for img_dir in mag_img_dir_lst:
mag_img_list.append(cv2.imread(img_dir))
ret, K2, D2 = cal_internal_monocular(obj_p, mag_img_list, checker_board)
# joint calibrate outside
obj_points, corner_success = cal_world_coordinates(orbbec_img_list, checker_board, square_size, orbbec_img_dir_lst, lib_path, camera_type='orbbec', tran_matrix=tran_matrix)
img = []
for idx, img_dir in enumerate(orbbec_img_dir_lst):
if corner_success[idx]:
img.append(cv2.imread(img_dir))
ret, rvec1, R1, T1 = cal_outside_image_monocular(obj_points, img, checker_board, K1, D1)
obj_points, corner_success = cal_world_coordinates(mag_img_list, checker_board, square_size, mag_img_dir_lst, lib_path, camera_type='mag', obj_orbbec=obj_points, corner_success=corner_success, tran_matrix=tran_matrix)
img = []
for idx, img_dir in enumerate(mag_img_dir_lst):
if corner_success[idx]:
img.append(cv2.imread(img_dir))
ret, rvec2, R2, T2 = cal_outside_image_monocular(obj_points, img, checker_board, K2, D2)
if not os.path.exists(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter")):
os.makedirs(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter"))
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/K1.txt"), K1)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/D1.txt"), D1)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/rvec1.txt"), rvec1)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/R1.txt"), R1)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/T1.txt"), T1)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/K2.txt"), K2)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/D2.txt"), D2)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/rvec2.txt"), rvec2)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/R2.txt"), R2)
np.savetxt(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter/T2.txt"), T2)
return K1, D1, rvec1, R1, T1, K2, D2, rvec2, R2, T2
# python orbbec_mag_joint_calibration.py --checker_board 6 9 --square_size 28 28 --lib_path C:\Users\38698\work_space\OpenNI\Win64-Release\sdk\libs --sample_path D:\data\hand_camera\1676431921 --tran_matrix C:\Users\38698\work_space\camera_joint_calibration\joint_parameter\tran_matrix.txt
if __name__ == '__main__':
# calibration
K1, D1, rvec1, R1, T1, K2, D2, rvec2, R2, T2 = joint_calibration()
K1, D1, rvec1, R1, T1, K2, D2, rvec2, R2, T2 = load_joint_parameter(os.path.join(PROJECT_ABSOLUTE_PATH, "joint_parameter"))
print("K1:", K1)
print("D1:", D1)
print("rvec1:", rvec1)
print("R1:", R1)
print("T1:", T1)
print("K2:", K2)
print("D2:", D2)
print("rvec2:", rvec2)
print("R2:", R2)
print("T2:", T2)