§ 2.8 像素坐标转换为三维空间坐标

1. 导入依赖

# 将astra.py所在的目录添加到系统路径里面
# 这样Python可以找到这个包
import sys
sys.path.append("../astra-open3d-python/")

# - 矩阵运算
import numpy as np
# - 图像处理
import cv2 
# - 绘图可视化
from matplotlib import pyplot as plt

# 自定义库
# - Astra 3D相机类
from astra import Astra 

2. 相机初始化

配置视频流格式为:彩图+深度图

# 创建相机对象
# config_path要写的是Astra 3D相机的配置文件夹路径
camera = Astra(config_path="../astra-open3d-python/config/")
# 初始相机视频流
camera.init_video_stream(video_mode="color_depth")
# 读取相机参数(相机内参)
camera.load_cam_calib_data()

3. 图像读取

获取彩图与深度图

# 获取彩图
color_img = camera.read_color_img()
# 获取深度图
depth_img = camera.read_depth_img()
# 生成画布
color_canvas = np.copy(color_img)
depth_canvas = camera.depth_img2canvas(depth_img)

4. 可视化

定义像素坐标

# 定义像素坐标
px = 400
py = 200

在彩图与深度图上进行位置标注。

radius = 10 # 像素半径
color = [0, 0, 255] # 颜色
thickness = 5 # 宽度
# 绘制圆圈
color_canvas = cv2.circle(color_canvas, [px, py], radius, color, thickness)
depth_canvas = cv2.circle(depth_canvas, [px, py], radius, color, thickness)

使用Matplotlib进行可视化。

plt.figure(figsize=(12, 8))
plt.subplot(1, 2, 1)
plt.title("Color")
plt.imshow(color_canvas[:, :, ::-1])

plt.subplot(1, 2, 2)
plt.title("Depth")

plt.imshow(depth_canvas[:, :, ::-1])

plt.savefig("data/pixel2point3d/compare.png")

5. 像素坐标转换为三维空间坐标

将像素坐标转换为RGB相机坐标系下的三维坐标

depth_value = depth_img[py, px]
print(f"深度值: {depth_value} mm")

输出日志:

深度值: 737.0 mm
cam_point3d = camera.rgb_camera.depth_pixel2cam_point3d(\
                                px, py, depth_value=depth_value)
cam_x, cam_y, cam_z = cam_point3d
print(f"彩色相机坐标系下的坐标: [{cam_x:.1f}, {cam_y:.1f}, {cam_z:.1f}], 单位mm")

输出日志:

彩色相机坐标系下的坐标: [128.1, -50.5, 737.0], 单位mm

6. 三维空间坐标动态更新

鼠标在彩图上进行移动,在深度图的区域显示该点在彩色相机坐标系下的坐标。

python pixel2point3d.py

pixel2point3d.py

'''
Astra 3D相机 - 像素转换为三维空间坐标
----------------------------------------------
@作者: 阿凯爱玩机器人
@QQ: 244561792
@微信: xingshunkai
@邮箱: xingshunkai@qq.com
@网址: deepsenserobot.com
@B站: "阿凯爱玩机器人"
'''
# 将astra.py所在的目录添加到系统路径里面
# 这样Python可以找到这个包
import sys
sys.path.append("../astra-open3d-python/")

# - 矩阵运算
import numpy as np
# - 图像处理
import cv2 
# - 绘图可视化
from matplotlib import pyplot as plt

# 自定义库
# - Astra 3D相机类
from astra import Astra 
# 配置项
# - 深度图对齐
image_registration = True
# 创建相机对象
# config_path要写的是Astra 3D相机的配置文件夹路径
camera = Astra(config_path="../astra-open3d-python/config/")
try:
	# 初始相机视频流
	camera.init_video_stream(video_mode="color_depth", \
	 	image_registration=image_registration)
except Exception as e:
	print("[错误] 没有发现Astra设备, 请检查接线或驱动")
	print(e)
	exit(-1)
# 读取相机参数(相机内参)
camera.load_cam_calib_data()

# 创建窗口
win_flag = cv2.WINDOW_NORMAL | cv2.WINDOW_KEEPRATIO | cv2.WINDOW_GUI_EXPANDED
cv2.namedWindow("color", flags=win_flag)
cv2.namedWindow("depth", flags=win_flag)

px = 0
py = 0
# 鼠标回调事件
def on_mouse(event,x,y,flags,param):
	global px, py
	if event == cv2.EVENT_MOUSEMOVE:
		# 鼠标移动事件
		px = x
		py = y
# 将on_mouse回调事件绑定到窗口image_win上
cv2.setMouseCallback("color", on_mouse)

while True:
	# 采集彩图
	color_img = camera.read_color_img()
	# 采集深度图
	depth_img = camera.read_depth_img() 
	# 生成画布
	color_canvas = np.copy(color_img)
	depth_canvas = camera.depth_img2canvas(depth_img)
	# 绘制圆圈
	radius = 10 # 像素半径
	color = [0, 0, 255] # 颜色
	thickness = 5 # 宽度
	# 绘制圆圈
	color_canvas = cv2.circle(color_canvas, [px, py], radius, color, thickness)
	depth_canvas = cv2.circle(depth_canvas, [px, py], radius, color, thickness)
	# 像素坐标
	cv2.putText(depth_canvas, text=f'PX: {px} PY: {py}',\
			org=(20, 30), fontFace=cv2.FONT_HERSHEY_SIMPLEX, \
			fontScale=1, thickness=2, lineType=cv2.LINE_AA, color=(0, 0, 255))
 	# 计算三维坐标
	depth_value = depth_img[py, px]
	
	if depth_value != 0:
		print(f"深度值: {depth_value} mm")
    	# 有效深度
		cam_point3d = camera.rgb_camera.depth_pixel2cam_point3d(\
									px, py, depth_value=depth_value)
		cam_x, cam_y, cam_z = cam_point3d
		print(f"彩色相机坐标系下的坐标: [{cam_x:.1f}, {cam_y:.1f}, {cam_z:.1f}], 单位mm")
		# 在画布上绘制三维坐标
		cv2.putText(depth_canvas, text=f'Camera: X {cam_x:.1f} Y {cam_y:.1f} Z {cam_z:.1f}',\
			org=(20, 60), fontFace=cv2.FONT_HERSHEY_SIMPLEX, \
			fontScale=1, thickness=2, lineType=cv2.LINE_AA, color=(0, 0, 255))
	else:
		print("深度值无效")
	
	# 显示图像
	cv2.imshow('color', color_canvas)
	cv2.imshow('depth', depth_canvas)
	
	key = cv2.waitKey(1)
	if key == ord('q'):
		# 如果按键为q 代表quit 退出程序
		break

# 关闭摄像头
camera.release()
# 销毁所有的窗口
cv2.destroyAllWindows()