def test_camera_coor(self):
point = [320, 240]
while True:
aligned_frames = self.get_aligned_frames()
depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
color_image = np.asanyarray(color_frame.get_data())
cv2.circle(color_image, (point[0], point[1]), 3, [0, 0, 255])
# Show images
cv2.namedWindow('Color image', cv2.WINDOW_AUTOSIZE)
cv2.imshow('Color image', color_image)
coor = self.pixelxy2cameraXYZ(aligned_frames, point)
print(coor)
# depth = (depth_frame.get_distance(point[0], point[1])) * 100
# print("depth=", depth)
# print("delta depth=(cm)", depth - coor[2])
key = cv2.waitKey(500)
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
Printer.print('Quit View', Printer.green)
break
elif key == ord('a'):
point[0] -= 1
elif key == ord('d'):
point[0] += 1
elif key == ord('s'):
point[1] += 1
elif key == ord('w'):
point[1] -= 1
def rigid_transform_3D(A: np.array, B: np.array):
""" B = RA + t """
N = A.shape[0]
# centroid_A, B
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
centroid_A = centroid_A.reshape((1, 3))
centroid_B = centroid_B.reshape((1, 3))
AA = A - np.tile(centroid_A, (N, 1))
BB = B - np.tile(centroid_B, (N, 1))
H = np.matmul(AA.T, BB)
U, S, V = np.linalg.svd(H)
R = np.matmul(V.T, U.T)
if np.linalg.det(R) < 0:
Printer.print('Reflection Detected.', Printer.red)
V[:, 2] *= -1
R = np.matmul(V.T, U.T)
t = centroid_B.T - np.matmul(R, centroid_A.T)
return R, t
def view_aligned_filled_images(self):
"""简单查看对齐的RGB和深度图像的状态"""
try:
while True:
align_frames = self.get_aligned_frames()
color_frame = align_frames.get_color_frame()
color_image = np.asanyarray(color_frame.get_data())
depth_frame_pro = self._filter_depth_frame(
align_frames.get_depth_frame())
depth_image_pro = np.asanyarray(depth_frame_pro.get_data())
depth_colormap_pro = cv2.applyColorMap(
cv2.convertScaleAbs(depth_image_pro, alpha=0.03),
cv2.COLORMAP_JET)
images = np.hstack((color_image, depth_colormap_pro))
# Show images
cv2.namedWindow('Color and aligned filled depth',
cv2.WINDOW_AUTOSIZE)
cv2.imshow('Color and aligned filled depth', images)
key = cv2.waitKey(1)
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
Printer.print('Quit View', Printer.green)
return
except cv2.error as e:
Printer.print('View aligned filled image error: %s' % e.__str__(),
Printer.red)
def load_control_point_files(self, path: str):
self._cp_coor_dic.clear()
self._cp_image_dic.clear()
# 读坐标文件
coor_filename = path + 'coors.txt'
file = open(coor_filename)
content_ls = file.readlines()
for line in content_ls:
if '' == line: # 空行
continue
line.replace(' ', '') # 空格
try:
pname, X, Y, Z = line.split(',')
X = float(X)
Y = float(Y)
Z = float(Z)
self._cp_coor_dic[pname] = np.asarray([X, Y, Z])
except ValueError as e:
Printer.print("Control point file ERROR." + e.__str__(),
Printer.red)
# 读模板图片
img_name_ls = glob.glob(path + '*.jpg')
for img_name in img_name_ls:
self._cp_image_dic[img_name[len(path):-4]] = cv2.imread(img_name)
return
def get_transform_matrix(self,
aligned_frames,
sigma_threshold=10) -> np.array:
self._found_point_XYZc_dic.clear()
# 检测目标点位置
image = self.get_color_image_from_frames(aligned_frames)
point_xy_dic, display_img = self._find_markers(image)
self.point_xy_dic = point_xy_dic # TODO 测试用
# 转成相机空间坐标
for pname, xy in point_xy_dic.items():
XYZ = self.pixelxy2cameraXYZ(aligned_frames, xy)
if XYZ is not None:
self._found_point_XYZc_dic[pname] = XYZ
# 解转换参数
# if self._solve_7_paras(sigma_threshold=sigma_threshold):
# if self._solve_perspect_M():
# if self._solve_para_4():
if self._solve_shift():
self.if_get_position_flag = True
Printer.print('transform matrix get.', Printer.green)
else:
self.if_get_position_flag = False
return display_img
def _update_found_points(self, found_points) -> bool:
"""
添加匹配到的点坐标
:param found_points: # [[点名: str, [Camera坐标X1, Y1, Z1], [Robot坐标X2, Y2, Z2]]]
:return: 成功与否bool
"""
# [[点名: str, [Camera坐标x1, y1, z1], [Robot坐标x2, y2, z2]]]
self._found_point_data.clear()
def find_control(pname: str):
for p in self.control_points:
if p[0] == pname:
return p
return None
for p in found_points:
if isinstance(p[0], str) and 3 == p[1].__len__():
control_point = find_control(p[0])
self._found_point_data.append(
[p[0], control_point[1],
np.asarray(p[1])])
else:
Printer.print("%s error: " % sys._getframe().f_code.co_name,
Printer.red)
return False
return True
def __init__(self):
# 初始化窗口
super().__init__()
self.setupUi(self)
# 定义线程
self.mainThread = MainThread()
self.monitor = MonitorThread(self.mainThread)
# 绑定信号
self.monitor.img_signal.connect(self._on_update_image_display)
self.monitor.voice_ready_signal.connect(self._on_voice_ready)
self.startButton.clicked.connect(self._on_start_button_clicked)
self.buttonVoice.clicked.connect(self._on_voice_button_clicked)
self.buttonCalibrate.clicked.connect(self._on_calibrate_button_clicked)
self.exitButton.clicked.connect(self._on_exit_button_clicked)
# 启动monitor
self.monitor.start()
self.buttonVoice.setEnabled(False)
# self._on_start_button_clicked()
self.show()
Printer.print("Window ready", Printer.green)
def _on_start_button_clicked(self):
print('start button clicked')
self.startButton.setEnabled(False)
self.mainThread.start()
Printer.print('Main thread started.', Printer.green)
self.startButton.setText('Started')
self.buttonVoice.setEnabled(True)
QApplication.processEvents()
def _solve_7_paras(self, sigma_threshold) -> bool:
n_pt = self._found_point_XYZc_dic.__len__()
n_equ = n_pt * 3 # 方程数
if n_equ < 6: # 点数不够
# Printer.print("Control point not enough. Can't solve.", Printer.yellow)
return False
B = np.zeros((n_equ, 6), dtype=np.float)
L = np.zeros((n_equ, 1), dtype=np.float)
# 每个点对应三个方程
i = 0
for pname, XYZ1 in self._found_point_XYZc_dic.items():
xyz2 = self._cp_coor_dic[pname]
x1, y1, z1 = XYZ1
x2, y2, z2 = xyz2
B[3 * i, :] = [1, 0, 0, 0, -z1, y1]
B[3 * i + 1, :] = [0, 1, 0, z1, 0, -x1]
B[3 * i + 2, :] = [0, 0, 1, -y1, x1, 0]
L[3 * i:3 * i + 3, 0] = [x2 - x1, y2 - y1, z2 - z1]
i += 1
# 解方程
try:
# B = B * -1
NBB = np.matmul(B.transpose(), B)
NBB_inv = np.linalg.inv(NBB)
BTPL = np.matmul(B.transpose(), L)
result = np.matmul(NBB_inv, BTPL)
self.trans_para_7[:, 0] = result[:, 0]
V = np.matmul(B, result) - L
r = n_equ - 6
VTV = np.matmul(V.transpose(), V)
sigma = sqrt(VTV[0, 0] / r)
print('sigma=', sigma)
if sigma > sigma_threshold:
Printer.print("sigma > %f" % sigma_threshold, Printer.yellow)
return False
else:
return True
except Exception as e:
Printer.print('matrix error: ' + e.__str__(), Printer.red)
return False
def _on_update_image_display(self, image):
# print("update image")
try:
if isinstance(image, int):
return
w = self.imgLabel_r.width()
h = self.imgLabel_r.height()
print('w=%d, h=%h' % (w, h))
cv2.resize(image, (w, h))
bytesPerLine = 3 * w
self.qImg = QImage(image.data, w, h, bytesPerLine,
QImage.Format_RGB888).rgbSwapped()
# 将Qimage显示出来
self.imgLabel_r.setPixmap(QPixmap.fromImage(self.qImg))
except Exception as e:
Printer.print('update image error: ' + e.__str__(), Printer.red)
def get_aligned_frames(self):
"""
获取对齐的帧, 深度向颜色对齐, 失败返回None
:return: 对齐后的帧(frames)，获取失败返回None
"""
try:
frames = self.pipeline.wait_for_frames()
# 深度向颜色对齐
align_frames = self.align.process(frames)
return align_frames
except cv2.error as e:
Printer.print('Get aligned frames error: %s' % e.__str__(),
Printer.red)
return None
def _solve_perspect_M(self) -> bool:
if self.point_xy_dic.__len__() < 4:
return False
src_points = []
dst_points = []
for pname, xy in self.point_xy_dic.items():
src_points.append(xy)
dst_points.append(self._cp_coor_dic[pname][0:2])
try:
M = cv2.getPerspectiveTransform(src_points, dst_points)
self.perspective_matrix = M
return True
except Exception as e:
Printer.print('perspect M error: ', e.__str__, Printer.red)
return False
def _solve_7_paras(self) -> bool:
n_pt = self._found_point_data.__len__()
n_equ = n_pt * 3 # 方程数
if n_equ < 6: # 点数不够
# Printer.print("Control point not enough. Can't solve.", Printer.yellow)
return False
B = np.zeros((n_equ, 6), dtype=np.float)
L = np.zeros((n_equ, 1), dtype=np.float)
# 每个点对应三个方程
for i in range(n_pt):
p_name, xyz1, xyz2 = self._found_point_data[i]
x1, y1, z1 = xyz1
x2, y2, z2 = xyz2
B[3 * i, :] = [1, 0, 0, 0, -z1, y1]
B[3 * i + 1, :] = [0, 1, 0, z1, 0, -x1]
B[3 * i + 2, :] = [0, 0, 1, -y1, x1, 0]
L[3 * i:3 * i + 3, 0] = [x2 - x1, y2 - y1, z2 - z1]
# 解方程
try:
B = B * -1
NBB = np.matmul(B.transpose(), B)
NBB_inv = np.linalg.inv(NBB)
BTPL = np.matmul(B.transpose(), L)
result = np.matmul(NBB_inv, BTPL)
self.trans_para[:, 0] = result[:, 0]
V = np.matmul(B, result) - L
r = n_equ - 6
VTV = np.matmul(V.transpose(), V)
sigma = sqrt(VTV[0, 0] / r)
print('sigma=', sigma)
if sigma > 0.1:
Printer.print("sigma > 0.1", Printer.yellow)
return False
else:
return True
except Exception as e:
Printer.print(e.__str__(), Printer.red)
return False
def coor_trans(self, xyz1: np.array):
"""
正向坐标变换
:param xyz1: 相机坐标系的坐标xyz1
:return: 返回转换后的np.array(xyz)， 若转换失败返回None
"""
if not self.if_get_position:
Printer.print("No camera position, Cannot trans coor.",
Printer.red)
return None
xyz1 = np.asarray(xyz1)
if 3 != xyz1.size:
Printer.print("coor trans error: xyz size wrong.", Printer.red)
return None
x1, y1, z1 = xyz1
B = np.asarray([[1, 0, 0, 0, -z1, y1], [0, 1, 0, z1, 0, -x1],
[0, 0, 1, -y1, x1, 0]])
xyz2 = np.matmul(B, self.trans_para).reshape(3) + xyz1
return xyz2
def coor_trans_pixelxy2worldXYZ(self, aligned_frames: rs.frame, xy: np.array):
if not self.if_get_position_flag:
# Printer.print("No camera position, Cannot trans coor.", Printer.red)
return None
if len(xy) != 2:
Printer.print('coor trans len(xy) != 2', Printer.red)
return None
# 七参数
XYZ = self.pixelxy2cameraXYZ(aligned_frames, xy)
if XYZ is None:
return None
XYZ = np.asarray(XYZ)
XYZ = self._coor_trans_cameraXYZ2worldXYZ(XYZ)
if 3 != XYZ.size:
Printer.print("coor trans error: xyz size wrong.", Printer.red)
return None
return XYZ
def view_color_image(self):
"""简单查看一下RGB工作状态"""
try:
while True:
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
color_image = np.asanyarray(color_frame.get_data())
# Show images
cv2.namedWindow('Color image', cv2.WINDOW_AUTOSIZE)
cv2.imshow('Color image', color_image)
key = cv2.waitKey(1)
if key & 0xFF == ord('q') or key == 27:
cv2.destroyAllWindows()
Printer.print('Quit View', Printer.green)
return
except cv2.error as e:
Printer.print('View color image error: %s' % e.__str__(),
Printer.red)
def load_control_point_file(self, filename=None):
if filename:
fname = filename
else:
fname = self.coor_filename
self.control_points.clear()
file = open(fname)
content_ls = file.readlines()
for line in content_ls:
if '' == line:
continue
line.replace(' ', '')
pname, x, y, z = line.split(',')
try:
x = float(x)
y = float(y)
z = float(z)
self.control_points.append([pname, np.asarray([x, y, z])])
except ValueError as e:
Printer.print("control point file error.", Printer.red)
return False
return True
def __init__(self,
color_resolution=(1920, 1080),
depth_resolution=(1280, 720)):
try:
# TODO
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, depth_resolution[0],
depth_resolution[1], rs.format.z16, 30)
self.config.enable_stream(rs.stream.color, color_resolution[0],
color_resolution[1], rs.format.bgr8, 30)
# 配置文件
self.profile = self.pipeline.start(self.config)
# 深度传感器
self.depth_sensor = self.profile.get_device().first_depth_sensor()
self.depth_sensor.set_option(rs.option.visual_preset, 4)
# 设备设置
self.device = rs.context().query_devices()[0]
self.advnced_mode = rs.rs400_advanced_mode(self.device)
self.set_disparityShift(1000)
self.set_depthUnits(100)
# 深度标尺
self.depth_scale = self.depth_sensor.get_depth_scale()
print('depth_scale=', self.depth_scale)
# 创建align对象
self.align_to = rs.stream.color
self.align = rs.align(self.align_to)
Printer.print("Camera ready", Printer.green)
except RuntimeError as e:
Printer.print("Camera init fail: " + e.__str__(), Printer.red)
def pixelxy2cameraXYZ(self, align_frames, pixel_coor):
"""
获取指定像素点[x, y](必须是List), 在相机坐标系下的三维坐标[X, Y, Z]
:param align_frames: 对齐的帧
:param pixel_coor: 目标像素的坐标, 只能传入list， 不能是array
:return:
"""
pixel_coor = np.asarray(np.round(pixel_coor), dtype=np.int)
# 获得对齐后的深度帧
align_depth_frame = align_frames.get_depth_frame()
color_frame = align_frames.get_color_frame()
if not color_frame or not align_depth_frame:
Printer.print('pixelxy2cameraXYZ error. no color_frame',
Printer.red)
return None
# 获得颜色帧的内参
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
align_depth_image = np.asanyarray(align_depth_frame.get_data())
try:
# 长度单位为cm， 图像寻址先行后列
align_depth_value = align_depth_image[
pixel_coor[1],
pixel_coor[0]] * self.depth_scale * 100 # 单位转换，100为cm，1为m
# 输入传感器内部参数、点的像素坐标、对应的深度值，输出点的三维坐标。
XYZ = rs.rs2_deproject_pixel_to_point(color_intrin,
list(pixel_coor),
align_depth_value)
# 判断结果是否异常
if [0, 0, 0] == XYZ:
Printer.print("Coor error, maybe too near", Printer.red)
return None
else:
# 坐标系变换
XYZ[2] *= -1
temp = XYZ[0]
XYZ[0] = XYZ[1]
XYZ[1] = temp
return XYZ
except Exception as e:
Printer.print('pixelxy2cameraXYZ error' + e.__str__(), Printer.red)
return None
def _on_exit_button_clicked(self):
self.mainThread.quit() # 退出线程 TODO 改成MainThread的
self.monitor.quit()
self.close() # 关闭窗口
Printer.print("exit", Printer.green)
def __init__(self, control_point_path='control_point_data/'):
super().__init__()
self.load_control_point_files(control_point_path)
Printer.print('control point files loaded.', Printer.green)
def _match_template_SIFT(self,
template_img,
target_kp,
target_des,
threshold,
draw_img=None,
min_match_count=25):
"""
用SIFT进行模板匹配，如果找到了返回center，如果没找到返回None
:param template_img:
:param target_kp:
:param target_des:
:param threshold:
:param draw_img:
:param min_match_count:
:return:
"""
# Initiate SIFT detector创建sift检测器
sift = cv2.xfeatures2d.SIFT_create()
# compute the descriptors with ORB
template_kp, template_des = sift.detectAndCompute(template_img, None)
# 创建设置FLANN匹配
FLANN_INDEX_KDTREE = 0
# 匹配算法
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
# 搜索次数
search_params = dict(checks=30)
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(template_des, target_des, k=2)
# store all the good matches as per Lowe's ratio test.
good_match = []
# 舍弃大于threshold的匹配, threshold越小越严格
for m, n in matches:
if m.distance < threshold * n.distance:
good_match.append(m)
if len(good_match) > min_match_count:
Printer.print(
"matches found - %d/%d" % (len(good_match), min_match_count),
Printer.green)
# 获取关键点的坐标
src_pts = np.float32([
template_kp[m.queryIdx].pt for m in good_match
]).reshape(-1, 1, 2)
dst_pts = np.float32([
target_kp[m.trainIdx].pt for m in good_match
]).reshape(-1, 1, 2)
# 计算变换矩阵和MASK
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
# matchesMask = mask.ravel().tolist()
h, w = template_img.shape[:2]
# 使用得到的变换矩阵对原图像的四个角进行变换，获得在目标图像上对应的坐标
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, M)
center = np.asarray([
np.round(np.mean(dst[:, 0, 0])),
np.round(np.mean(dst[:, 0, 1]))
],
dtype=np.int)
cv2.polylines(draw_img, [np.int32(dst)], True, [0, 0, 255], 2,
cv2.LINE_AA)
cv2.circle(draw_img, (center[0], center[1]), 3, (0, 255, 0), 3)
else:
Printer.print(
"matches found - %d/%d" % (len(good_match), min_match_count),
Printer.yellow)
center = None
return center