pcl-python官方文档:
http://www.dabeaz.com/ply/ply.html
建议大家到官网下载,直接利用源代码安装
sudo python setup.py install
偷懒一点的朋友,也可以直接用apt安装,
sudo apt-get install python-ply
官方py文档:pyrealsense2
dianyunxianshi:
https://github.com/dorodnic/binder_test/blob/master/pointcloud.ipynb
上一篇文章
https://blog.csdn.net/baidu_40840693/article/details/102918931
https://github.com/IntelRealSense/librealsense/tree/master/wrappers/python/examples
python -mpip install pyglet==1.3.0
python -mpip install pyrealsense2
python -mpip install pyntcloud
官方例子
https://github.com/IntelRealSense/librealsense/blob/jupyter/notebooks/depth_filters.ipynb
https://github.com/IntelRealSense/librealsense/blob/jupyter/notebooks/distance_to_object.ipynb
https://github.com/IntelRealSense/librealsense/tree/jupyter/notebooks
下载文件:
http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/object_detection.bag
bag我没用上,因为bag是该设备保存的格式,代码中的
cfg.enable_device_from_file("../object_detection.bag")
是载入这个视频的意思,我们也可以保存,方式是
https://github.com/IntelRealSense/librealsense/issues/3029
config = rs.config()
config.enable_record_to_file('test.bag')
http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/MobileNetSSD_deploy.caffemodel
例子:
import cv2 # state of the art computer vision algorithms library
import numpy as np # fundamental package for scientific computing
import matplotlib.pyplot as plt # 2D plotting library producing publication quality figures
import pyrealsense2 as rs # Intel RealSense cross-platform open-source API
print("Environment Ready")
# Setup:
pipe = rs.pipeline()
cfg = rs.config()
#cfg.enable_device_from_file("../object_detection.bag")
cfg.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30)
#cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
cfg.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
profile = pipe.start(cfg)
# Skip 5 first frames to give the Auto-Exposure time to adjust
for x in range(100):
pipe.wait_for_frames()
# Store next frameset for later processing:
frameset = pipe.wait_for_frames()
color_frame = frameset.get_color_frame()
depth_frame = frameset.get_depth_frame()
# Cleanup:
pipe.stop()
print("Frames Captured")
color = np.asanyarray(color_frame.get_data())
plt.rcParams["axes.grid"] = False
plt.rcParams['figure.figsize'] = [12, 6]
plt.imshow(color)
plt.show()
colorizer = rs.colorizer()
colorized_depth = np.asanyarray(colorizer.colorize(depth_frame).get_data())
plt.imshow(colorized_depth)
plt.show()
# Create alignment primitive with color as its target stream:
align = rs.align(rs.stream.color)
frameset = align.process(frameset)
# Update color and depth frames:
aligned_depth_frame = frameset.get_depth_frame()
colorized_depth = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
# Show the two frames together:
images = np.hstack((color, colorized_depth))
plt.imshow(images)
plt.show()
# Standard OpenCV boilerplate for running the net:
height, width = color.shape[:2]
expected = 300
aspect = width / height
resized_image = cv2.resize(color, (round(expected * aspect), expected))
crop_start = round(expected * (aspect - 1) / 2)
crop_img = resized_image[0:expected, crop_start:crop_start+expected]
net = cv2.dnn.readNetFromCaffe("./MobileNetSSD_deploy.prototxt", "./MobileNetSSD_deploy.caffemodel")
inScaleFactor = 0.007843
meanVal = 127.53
classNames = ("background", "aeroplane", "bicycle", "bird", "boat",
"bottle", "bus", "car", "cat", "chair",
"cow", "diningtable", "dog", "horse",
"motorbike", "person", "pottedplant",
"sheep", "sofa", "train", "tvmonitor")
blob = cv2.dnn.blobFromImage(crop_img, inScaleFactor, (expected, expected), meanVal, False)
net.setInput(blob, "data")
detections = net.forward("detection_out")
label = detections[0,0,0,1]
conf = detections[0,0,0,2]
xmin = detections[0,0,0,3]
ymin = detections[0,0,0,4]
xmax = detections[0,0,0,5]
ymax = detections[0,0,0,6]
className = classNames[int(label)]
cv2.rectangle(crop_img, (int(xmin * expected), int(ymin * expected)),
(int(xmax * expected), int(ymax * expected)), (255, 255, 255), 2)
cv2.putText(crop_img, className,
(int(xmin * expected), int(ymin * expected) - 5),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255))
plt.imshow(crop_img)
plt.show()
scale = height / expected
xmin_depth = int((xmin * expected + crop_start) * scale)
ymin_depth = int((ymin * expected) * scale)
xmax_depth = int((xmax * expected + crop_start) * scale)
ymax_depth = int((ymax * expected) * scale)
xmin_depth,ymin_depth,xmax_depth,ymax_depth
cv2.rectangle(colorized_depth, (xmin_depth, ymin_depth),
(xmax_depth, ymax_depth), (255, 255, 255), 2)
plt.imshow(colorized_depth)
plt.show()
depth = np.asanyarray(aligned_depth_frame.get_data())
# Crop depth data:
depth = depth[xmin_depth:xmax_depth,ymin_depth:ymax_depth].astype(float)
# Get data scale from the device and convert to meters
depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
depth = depth * depth_scale
dist,_,_,_ = cv2.mean(depth)
print("Detected a {0} {1:.3} meters away.".format(className, dist))
注释修改版:
# -*-coding:utf-8-*-
import cv2 # state of the art computer vision algorithms library
import numpy as np # fundamental package for scientific computing
import matplotlib.pyplot as plt # 2D plotting library producing publication quality figures
import pyrealsense2 as rs # Intel RealSense cross-platform open-source API
print("Environment Ready")
# 创建一个管道
pipe = rs.pipeline()
# 配置要流式传输的管道
# 颜色和深度流的不同分辨率
cfg = rs.config()
#cfg.enable_device_from_file("../object_detection.bag")
#1280, 720
cfg.enable_stream(rs.stream.depth, 640, 360, rs.format.z16, 30)
#cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
cfg.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
# 开始流式传输
profile = pipe.start(cfg)
# 跳过前300帧以设置自动曝光时间
for x in range(100):
pipe.wait_for_frames()
# 拿到301帧
frameset = pipe.wait_for_frames()
# RGB图
color_frame = frameset.get_color_frame()
# 深度图
depth_frame = frameset.get_depth_frame()
# 停止管道传输
pipe.stop()
print("Frames Captured")
# 显示RGB颜色图像color_frame
color = np.asanyarray(color_frame.get_data())
# 一些可视化显示设置,不用管
plt.rcParams["axes.grid"] = False
plt.rcParams['figure.figsize'] = [12, 6]
plt.imshow(color)
plt.show()
# 显示深度图
# (360, 640) uint16
depth_image_ori = np.asanyarray(depth_frame.get_data())
plt.imshow(depth_image_ori)
plt.show()
# https://github.com/IntelRealSense/librealsense/issues/3665
# 不使用int8是因为精度不够cm
depth_image_ori_int8 = depth_image_ori.astype('uint8')
plt.imshow(depth_image_ori_int8)
plt.show()
# 创建colorizer滤波对象 将深度图映射成彩色图像显示
colorizer = rs.colorizer()
colorized_depth = np.asanyarray(colorizer.colorize(depth_frame).get_data())
plt.imshow(colorized_depth)
plt.show()
# 创建对齐对象
# rs.align允许我们执行深度帧与其他帧的对齐
# “frameset”是我们计划对齐深度帧的流类型。
# 将深度框与颜色框对齐
# 将深度对齐到颜色
align = rs.align(rs.stream.color)
frameset = align.process(frameset)
# 获取对齐更新后的深度图
aligned_depth_frame = frameset.get_depth_frame()
# 将深度图映射成彩色图像显示
colorized_depth = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
# np.vstack():在竖直方向上堆叠
# np.hstack():在水平方向上平铺
images = np.hstack((color, colorized_depth))
plt.imshow(images)
plt.show()
# np.vstack():在竖直方向上堆叠
# np.hstack():在水平方向上平铺
images = np.vstack((color, colorized_depth))
plt.imshow(images)
plt.show()
# 通过对齐后的深度图,对齐原始RGB:color_frame,保存彩色点云
pc = rs.pointcloud()
pc.map_to(color_frame)
points = pc.calculate(aligned_depth_frame)
points.export_to_ply('./out.ply', color_frame)
#pcd = read_point_cloud(file_path)
# Visualize PLY
#draw_geometries([pcd])
#pc.map_to(colorizer.colorize(aligned_depth_frame))
#points.export_to_ply('./out.ply', colorizer.colorize(aligned_depth_frame))
# 480*640*3
height, width = color.shape[:2]
expected = 300
# 1.333333... 原始图像的宽高比
aspect = width / height
# 300*400*3 新的图像也满足此宽高比
resized_image = cv2.resize(color, (round(expected * aspect), expected))
# 50
crop_start = round(expected * (aspect - 1) / 2)
# 300*300*3 H,W
crop_img = resized_image[0:expected, crop_start:crop_start+expected]
plt.imshow(crop_img)
plt.show()
# VOC目标检测网络
net = cv2.dnn.readNetFromCaffe("./MobileNetSSD_deploy.prototxt", "./MobileNetSSD_deploy.caffemodel")
inScaleFactor = 0.007843
meanVal = 127.53
classNames = ("background", "aeroplane", "bicycle", "bird", "boat",
"bottle", "bus", "car", "cat", "chair",
"cow", "diningtable", "dog", "horse",
"motorbike", "person", "pottedplant",
"sheep", "sofa", "train", "tvmonitor")
blob = cv2.dnn.blobFromImage(crop_img, inScaleFactor, (expected, expected), meanVal, False)
net.setInput(blob, "data")
# 1*1*100*7[1-6]
detections = net.forward("detection_out")
# 输出检测结果
# label = detections[0,0,0,1]
# conf = detections[0,0,0,2]
# xmin = detections[0,0,0,3]
# ymin = detections[0,0,0,4]
# xmax = detections[0,0,0,5]
# ymax = detections[0,0,0,6]
# className = classNames[int(label)]
# cv2.rectangle(crop_img, (int(xmin * expected), int(ymin * expected)),
# (int(xmax * expected), int(ymax * expected)), (255, 255, 255), 2)
# cv2.putText(crop_img, className,
# (int(xmin * expected), int(ymin * expected) - 5),
# cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255))
# plt.imshow(crop_img)
# plt.show()
# scale = height / expected
# xmin_depth = int((xmin * expected + crop_start) * scale)
# ymin_depth = int((ymin * expected) * scale)
# xmax_depth = int((xmax * expected + crop_start) * scale)
# ymax_depth = int((ymax * expected) * scale)
# xmin_depth,ymin_depth,xmax_depth,ymax_depth
# cv2.rectangle(colorized_depth, (xmin_depth, ymin_depth),
# (xmax_depth, ymax_depth), (255, 255, 255), 2)
# plt.imshow(colorized_depth)
# plt.show()
label1 = detections[0,0,:,1]
conf1 = detections[0,0,:,2]
xmin1 = detections[0,0,:,3]
ymin1 = detections[0,0,:,4]
xmax1 = detections[0,0,:,5]
ymax1 = detections[0,0,:,6]
# 获取满足阈值的框
# 框的x1y1 x2y2 是百分比,相对于300*300
inds = np.where(conf1[:] > 0.3)[0]
for index in inds:
className = classNames[int(label1[index])]
cv2.rectangle(crop_img, (int(xmin1[index] * expected), int(ymin1[index] * expected)),
(int(xmax1[index] * expected), int(ymax1[index] * expected)), (255, 255, 255), 2)
cv2.putText(crop_img, className,
(int(xmin1[index] * expected), int(ymin1[index] * expected) - 5),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255))
plt.imshow(crop_img)
plt.show()
# 对于300,原始图像的扩大比为多少
# 对齐后的深度图:aligned_depth_frame
# 对齐后的深度图的彩色图:colorized_depth
# xmin1[index] * expected 300*300检测图的坐标
# xmin1[index] * expected + crop_start 300*400*3的坐标
# (xmin1[index] * expected + crop_start) * scale 480*640*3的坐标
depth = np.asanyarray(aligned_depth_frame.get_data())
depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
aa = profile.get_device()
scale = height / expected
from decimal import Decimal
for index in inds:
xmin_depth = int((xmin1[index] * expected + crop_start) * scale)
ymin_depth = int((ymin1[index] * expected) * scale)
xmax_depth = int((xmax1[index] * expected + crop_start) * scale)
ymax_depth = int((ymax1[index] * expected) * scale)
#xmin_depth,ymin_depth,xmax_depth,ymax_depth
#depth_temp = depth[xmin_depth:xmax_depth, ymin_depth:ymax_depth].astype(float)
depth_temp = depth[ymin_depth:ymax_depth, xmin_depth:xmax_depth].astype(float)
depth_temp = depth_temp * depth_scale
dist, _, _, _ = cv2.mean(depth_temp)
cv2.rectangle(colorized_depth, (xmin_depth, ymin_depth),
(xmax_depth, ymax_depth), (255, 255, 255), 2)
# 取小数点后3位
dist_temp = Decimal(dist).quantize(Decimal('0.000'))
cv2.putText(colorized_depth, str(dist_temp),
(xmin_depth, ymin_depth - 5),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255))
plt.imshow(colorized_depth)
plt.show()
plt.close()
# # 对齐后的深度图:aligned_depth_frame
# # 对齐后的深度图的彩色图:colorized_depth
# depth = np.asanyarray(aligned_depth_frame.get_data())
# # crop深度数据
# depth = depth[xmin_depth:xmax_depth,ymin_depth:ymax_depth].astype(float)
# # 计算深度
# # 设备获取数据比例并转换为m
# depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
# depth = depth * depth_scale
# # 取均值获取到物体的距离
# dist,_,_,_ = cv2.mean(depth)
# print("Detected a {0} {1:.3} meters away.".format(className, dist))
改一改:
# -*-coding:utf-8-*-
import cv2 # state of the art computer vision algorithms library
import numpy as np # fundamental package for scientific computing
import matplotlib.pyplot as plt # 2D plotting library producing publication quality figures
import pyrealsense2 as rs # Intel RealSense cross-platform open-source API
print("Environment Ready")
# 创建一个管道
pipe = rs.pipeline()
# 配置要流式传输的管道
# 颜色和深度流的不同分辨率
cfg = rs.config()
#cfg.enable_device_from_file("./object_detection.bag")
#cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
#cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
#1280, 720
cfg.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
#cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
cfg.enable_stream(rs.stream.color, 1280, 720, rs.format.rgb8, 30)
# 开始流式传输
profile = pipe.start(cfg)
# 跳过前300帧以设置自动曝光时间
for x in range(100):
pipe.wait_for_frames()
# 拿到301帧
frameset = pipe.wait_for_frames()
# RGB图
color_frame = frameset.get_color_frame()
# 深度图
depth_frame = frameset.get_depth_frame()
# 停止管道传输
pipe.stop()
print("Frames Captured")
# 获取内参外参矩阵
# Intrinsics & Extrinsics
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
# 外参矩阵-深度图相对于彩色图像的外参
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(
color_frame.profile)
print("内参ppx,ppy",depth_intrin.ppx, ':', depth_intrin.ppy)
print("内参矩阵",depth_intrin)
# 显示RGB颜色图像color_frame
color = np.asanyarray(color_frame.get_data())
# 一些可视化显示设置,不用管
plt.rcParams["axes.grid"] = False
plt.rcParams['figure.figsize'] = [12, 6]
plt.imshow(color)
plt.show()
# 显示深度图
# (360, 640) uint16
depth_image_ori = np.asanyarray(depth_frame.get_data())
plt.imshow(depth_image_ori)
plt.show()
# https://github.com/IntelRealSense/librealsense/issues/3665
# 不使用int8是因为精度不够cm
depth_image_ori_int8 = depth_image_ori.astype('uint8')
plt.imshow(depth_image_ori_int8)
plt.show()
# 创建colorizer滤波对象 将深度图映射成彩色图像显示
colorizer = rs.colorizer()
colorized_depth = np.asanyarray(colorizer.colorize(depth_frame).get_data())
plt.imshow(colorized_depth)
plt.show()
# 创建对齐对象
# rs.align允许我们执行深度帧与其他帧的对齐
# “frameset”是我们计划对齐深度帧的流类型。
# 将深度框与颜色框对齐
# 将深度对齐到颜色
align = rs.align(rs.stream.color)
frameset = align.process(frameset)
# 获取对齐更新后的深度图
aligned_depth_frame = frameset.get_depth_frame()
# 将深度图映射成彩色图像显示
colorized_depth = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
new_depth_image_array = np.asanyarray(aligned_depth_frame.get_data())
np.save("out_ori.npy",new_depth_image_array)
x = np.load("out_ori.npy")
print(x-new_depth_image_array)
bgr_color = cv2.cvtColor(color,cv2.COLOR_RGB2BGR)
cv2.imwrite("out_ori.jpg",bgr_color)
# np.vstack():在竖直方向上堆叠
# np.hstack():在水平方向上平铺
images = np.hstack((color, colorized_depth))
plt.imshow(images)
plt.show()
# np.vstack():在竖直方向上堆叠
# np.hstack():在水平方向上平铺
images = np.vstack((color, colorized_depth))
plt.imshow(images)
plt.show()
# 通过对齐后的深度图,对齐原始RGB:color_frame,保存彩色点云
pc = rs.pointcloud()
points = rs.points()
pc.map_to(color_frame)
points = pc.calculate(depth_frame)
points.export_to_ply('./out_ori.ply', color_frame)
#pcd = read_point_cloud(file_path)
# Visualize PLY
#draw_geometries([pcd])
#pc.map_to(colorizer.colorize(aligned_depth_frame))
#points.export_to_ply('./out.ply', colorizer.colorize(aligned_depth_frame))
# 480*640*3
height, width = color.shape[:2]
expected = 300
# 1.333333... 原始图像的宽高比
aspect = width / height
# 300*400*3 新的图像也满足此宽高比
resized_image = cv2.resize(color, (round(expected * aspect), expected))
# 50
crop_start = round(expected * (aspect - 1) / 2)
# 300*300*3 H,W
crop_img = resized_image[0:expected, crop_start:crop_start+expected]
plt.imshow(crop_img)
plt.show()
# VOC目标检测网络
net = cv2.dnn.readNetFromCaffe("./MobileNetSSD_deploy.prototxt", "./MobileNetSSD_deploy.caffemodel")
inScaleFactor = 0.007843
meanVal = 127.53
classNames = ("background", "aeroplane", "bicycle", "bird", "boat",
"bottle", "bus", "car", "cat", "chair",
"cow", "diningtable", "dog", "horse",
"motorbike", "person", "pottedplant",
"sheep", "sofa", "train", "tvmonitor")
blob = cv2.dnn.blobFromImage(crop_img, inScaleFactor, (expected, expected), meanVal, False)
net.setInput(blob, "data")
# 1*1*100*7[1-6]
detections = net.forward("detection_out")
# 输出检测结果
# label = detections[0,0,0,1]
# conf = detections[0,0,0,2]
# xmin = detections[0,0,0,3]
# ymin = detections[0,0,0,4]
# xmax = detections[0,0,0,5]
# ymax = detections[0,0,0,6]
# className = classNames[int(label)]
# cv2.rectangle(crop_img, (int(xmin * expected), int(ymin * expected)),
# (int(xmax * expected), int(ymax * expected)), (255, 255, 255), 2)
# cv2.putText(crop_img, className,
# (int(xmin * expected), int(ymin * expected) - 5),
# cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255))
# plt.imshow(crop_img)
# plt.show()
# scale = height / expected
# xmin_depth = int((xmin * expected + crop_start) * scale)
# ymin_depth = int((ymin * expected) * scale)
# xmax_depth = int((xmax * expected + crop_start) * scale)
# ymax_depth = int((ymax * expected) * scale)
# xmin_depth,ymin_depth,xmax_depth,ymax_depth
# cv2.rectangle(colorized_depth, (xmin_depth, ymin_depth),
# (xmax_depth, ymax_depth), (255, 255, 255), 2)
# plt.imshow(colorized_depth)
# plt.show()
label1 = detections[0,0,:,1]
conf1 = detections[0,0,:,2]
xmin1 = detections[0,0,:,3]
ymin1 = detections[0,0,:,4]
xmax1 = detections[0,0,:,5]
ymax1 = detections[0,0,:,6]
# 获取满足阈值的框
# 框的x1y1 x2y2 是百分比,相对于300*300
inds = np.where(conf1[:] > 0.3)[0]
for index in inds:
className = classNames[int(label1[index])]
cv2.rectangle(crop_img, (int(xmin1[index] * expected), int(ymin1[index] * expected)),
(int(xmax1[index] * expected), int(ymax1[index] * expected)), (255, 255, 255), 2)
cv2.putText(crop_img, className,
(int(xmin1[index] * expected), int(ymin1[index] * expected) - 5),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (255,255,255))
plt.imshow(crop_img)
plt.show()
# 对于300,原始图像的扩大比为多少
# 对齐后的深度图:aligned_depth_frame
# 对齐后的深度图的彩色图:colorized_depth
# xmin1[index] * expected 300*300检测图的坐标
# xmin1[index] * expected + crop_start 300*400*3的坐标
# (xmin1[index] * expected + crop_start) * scale 480*640*3的坐标
depth = np.asanyarray(aligned_depth_frame.get_data())
depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
aa = profile.get_device()
scale = height / expected
from decimal import Decimal
for index in inds:
xmin_depth = int((xmin1[index] * expected + crop_start) * scale)
ymin_depth = int((ymin1[index] * expected) * scale)
xmax_depth = int((xmax1[index] * expected + crop_start) * scale)
ymax_depth = int((ymax1[index] * expected) * scale)
depth_temp = depth[ymin_depth:ymax_depth, xmin_depth:xmax_depth].astype(float)
#print(depth_temp.shape)
depth_temp = depth_temp * depth_scale
#dis = depth_temp[depth_tem,depth_temp]
dist, _, _, _ = cv2.mean(depth_temp)
cv2.rectangle(colorized_depth, (xmin_depth, ymin_depth),
(xmax_depth, ymax_depth), (255, 255, 255), 2)
# 取小数点后3位
dist_temp = Decimal(dist).quantize(Decimal('0.000'))
cv2.putText(colorized_depth, str(dist_temp),
(xmin_depth, ymin_depth - 5),
cv2.FONT_HERSHEY_COMPLEX, 1.5, (255,255,255))
plt.imshow(colorized_depth)
plt.show()
# # 对齐后的深度图:aligned_depth_frame
# # 对齐后的深度图的彩色图:colorized_depth
# depth = np.asanyarray(aligned_depth_frame.get_data())
# # crop深度数据
# depth = depth[xmin_depth:xmax_depth,ymin_depth:ymax_depth].astype(float)
# # 计算深度
# # 设备获取数据比例并转换为m
# depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
# depth = depth * depth_scale
# # 取均值获取到物体的距离
# dist,_,_,_ = cv2.mean(depth)
# print("Detected a {0} {1:.3} meters away.".format(className, dist))
点云程序:
# License: Apache 2.0. See LICENSE file in root directory.
# Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
"""
OpenGL Pointcloud viewer with http://pyglet.org
Usage:
------
Mouse:
Drag with left button to rotate around pivot (thick small axes),
with right button to translate and the wheel to zoom.
Keyboard:
[p] Pause
[r] Reset View
[d] Cycle through decimation values
[z] Toggle point scaling
[x] Toggle point distance attenuation
[c] Toggle color source
[l] Toggle lighting
[f] Toggle depth post-processing
[s] Save PNG (./out.png)
[e] Export points to ply (./out.ply)
[q/ESC] Quit
Notes:
------
Using deprecated OpenGL (FFP lighting, matrix stack...) however, draw calls
are kept low with pyglet.graphics.* which uses glDrawArrays internally.
Normals calculation is done with numpy on CPU which is rather slow, should really
be done with shaders but was omitted for several reasons - brevity, for lowering
dependencies (pyglet doesn't ship with shader support & recommends pyshaders)
and for reference.
"""
import math
import ctypes
import pyglet
import pyglet.gl as gl
import numpy as np
import pyrealsense2 as rs
# https://stackoverflow.com/a/6802723
def rotation_matrix(axis, theta):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
axis = axis / math.sqrt(np.dot(axis, axis))
a = math.cos(theta / 2.0)
b, c, d = -axis * math.sin(theta / 2.0)
aa, bb, cc, dd = a * a, b * b, c * c, d * d
bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
[2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
[2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])
class AppState:
def __init__(self, *args, **kwargs):
self.pitch, self.yaw = math.radians(-10), math.radians(-15)
self.translation = np.array([0, 0, 1], np.float32)
self.distance = 2
self.mouse_btns = [False, False, False]
self.paused = False
self.decimate = 0
self.scale = True
self.attenuation = False
self.color = True
self.lighting = False
self.postprocessing = False
def reset(self):
self.pitch, self.yaw, self.distance = 0, 0, 2
self.translation[:] = 0, 0, 1
@property
def rotation(self):
Rx = rotation_matrix((1, 0, 0), math.radians(-self.pitch))
Ry = rotation_matrix((0, 1, 0), math.radians(-self.yaw))
return np.dot(Ry, Rx).astype(np.float32)
state = AppState()
# Configure streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
# other_stream, other_format = rs.stream.infrared, rs.format.y8
other_stream, other_format = rs.stream.color, rs.format.rgb8
config.enable_stream(other_stream, 640, 480, other_format, 30)
# Start streaming
pipeline.start(config)
profile = pipeline.get_active_profile()
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
depth_intrinsics = depth_profile.get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
# Processing blocks
pc = rs.pointcloud()
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
colorizer = rs.colorizer()
filters = [rs.disparity_transform(),
rs.spatial_filter(),
rs.temporal_filter(),
rs.disparity_transform(False)]
# pyglet
window = pyglet.window.Window(
config=gl.Config(
double_buffer=True,
samples=8 # MSAA
),
resizable=True, vsync=True)
keys = pyglet.window.key.KeyStateHandler()
window.push_handlers(keys)
def convert_fmt(fmt):
"""rs.format to pyglet format string"""
return {
rs.format.rgb8: 'RGB',
rs.format.bgr8: 'BGR',
rs.format.rgba8: 'RGBA',
rs.format.bgra8: 'BGRA',
rs.format.y8: 'L',
}[fmt]
# Create a VertexList to hold pointcloud data
# Will pre-allocates memory according to the attributes below
vertex_list = pyglet.graphics.vertex_list(
w * h, 'v3f/stream', 't2f/stream', 'n3f/stream')
# Create and allocate memory for our color data
other_profile = rs.video_stream_profile(profile.get_stream(other_stream))
image_data = pyglet.image.ImageData(w, h, convert_fmt(
other_profile.format()), (gl.GLubyte * (w * h * 3))())
fps_display = pyglet.clock.ClockDisplay()
@window.event
def on_mouse_drag(x, y, dx, dy, buttons, modifiers):
w, h = map(float, window.get_size())
if buttons & pyglet.window.mouse.LEFT:
state.yaw -= dx * 0.5
state.pitch -= dy * 0.5
if buttons & pyglet.window.mouse.RIGHT:
dp = np.array((dx / w, -dy / h, 0), np.float32)
state.translation += np.dot(state.rotation, dp)
if buttons & pyglet.window.mouse.MIDDLE:
dz = dy * 0.01
state.translation -= (0, 0, dz)
state.distance -= dz
def handle_mouse_btns(x, y, button, modifiers):
state.mouse_btns[0] ^= (button & pyglet.window.mouse.LEFT)
state.mouse_btns[1] ^= (button & pyglet.window.mouse.RIGHT)
state.mouse_btns[2] ^= (button & pyglet.window.mouse.MIDDLE)
window.on_mouse_press = window.on_mouse_release = handle_mouse_btns
@window.event
def on_mouse_scroll(x, y, scroll_x, scroll_y):
dz = scroll_y * 0.1
state.translation -= (0, 0, dz)
state.distance -= dz
def on_key_press(symbol, modifiers):
if symbol == pyglet.window.key.R:
state.reset()
if symbol == pyglet.window.key.P:
state.paused ^= True
if symbol == pyglet.window.key.D:
state.decimate = (state.decimate + 1) % 3
decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
if symbol == pyglet.window.key.C:
state.color ^= True
if symbol == pyglet.window.key.Z:
state.scale ^= True
if symbol == pyglet.window.key.X:
state.attenuation ^= True
if symbol == pyglet.window.key.L:
state.lighting ^= True
if symbol == pyglet.window.key.F:
state.postprocessing ^= True
if symbol == pyglet.window.key.S:
pyglet.image.get_buffer_manager().get_color_buffer().save('out.png')
if symbol == pyglet.window.key.Q:
window.close()
window.push_handlers(on_key_press)
def axes(size=1, width=1):
"""draw 3d axes"""
gl.glLineWidth(width)
pyglet.graphics.draw(6, gl.GL_LINES,
('v3f', (0, 0, 0, size, 0, 0,
0, 0, 0, 0, size, 0,
0, 0, 0, 0, 0, size)),
('c3f', (1, 0, 0, 1, 0, 0,
0, 1, 0, 0, 1, 0,
0, 0, 1, 0, 0, 1,
))
)
def frustum(intrinsics):
"""draw camera's frustum"""
w, h = intrinsics.width, intrinsics.height
batch = pyglet.graphics.Batch()
for d in range(1, 6, 2):
def get_point(x, y):
p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d)
batch.add(2, gl.GL_LINES, None, ('v3f', [0, 0, 0] + p))
return p
top_left = get_point(0, 0)
top_right = get_point(w, 0)
bottom_right = get_point(w, h)
bottom_left = get_point(0, h)
batch.add(2, gl.GL_LINES, None, ('v3f', top_left + top_right))
batch.add(2, gl.GL_LINES, None, ('v3f', top_right + bottom_right))
batch.add(2, gl.GL_LINES, None, ('v3f', bottom_right + bottom_left))
batch.add(2, gl.GL_LINES, None, ('v3f', bottom_left + top_left))
batch.draw()
def grid(size=1, n=10, width=1):
"""draw a grid on xz plane"""
gl.glLineWidth(width)
s = size / float(n)
s2 = 0.5 * size
batch = pyglet.graphics.Batch()
for i in range(0, n + 1):
x = -s2 + i * s
batch.add(2, gl.GL_LINES, None, ('v3f', (x, 0, -s2, x, 0, s2)))
for i in range(0, n + 1):
z = -s2 + i * s
batch.add(2, gl.GL_LINES, None, ('v3f', (-s2, 0, z, s2, 0, z)))
batch.draw()
@window.event
def on_draw():
window.clear()
gl.glEnable(gl.GL_DEPTH_TEST)
gl.glEnable(gl.GL_LINE_SMOOTH)
width, height = window.get_size()
gl.glViewport(0, 0, width, height)
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
gl.gluPerspective(60, width / float(height), 0.01, 20)
gl.glMatrixMode(gl.GL_TEXTURE)
gl.glLoadIdentity()
# texcoords are [0..1] and relative to top-left pixel corner, add 0.5 to center
gl.glTranslatef(0.5 / image_data.width, 0.5 / image_data.height, 0)
# texture size may be increased by pyglet to a power of 2
tw, th = image_data.texture.owner.width, image_data.texture.owner.height
gl.glScalef(image_data.width / float(tw),
image_data.height / float(th), 1)
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadIdentity()
gl.gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0)
gl.glTranslatef(0, 0, state.distance)
gl.glRotated(state.pitch, 1, 0, 0)
gl.glRotated(state.yaw, 0, 1, 0)
if any(state.mouse_btns):
axes(0.1, 4)
gl.glTranslatef(0, 0, -state.distance)
gl.glTranslatef(*state.translation)
gl.glColor3f(0.5, 0.5, 0.5)
gl.glPushMatrix()
gl.glTranslatef(0, 0.5, 0.5)
grid()
gl.glPopMatrix()
psz = max(window.get_size()) / float(max(w, h)) if state.scale else 1
gl.glPointSize(psz)
distance = (0, 0, 1) if state.attenuation else (1, 0, 0)
gl.glPointParameterfv(gl.GL_POINT_DISTANCE_ATTENUATION,
(gl.GLfloat * 3)(*distance))
if state.lighting:
ldir = [0.5, 0.5, 0.5] # world-space lighting
ldir = np.dot(state.rotation, (0, 0, 1)) # MeshLab style lighting
ldir = list(ldir) + [0] # w=0, directional light
gl.glLightfv(gl.GL_LIGHT0, gl.GL_POSITION, (gl.GLfloat * 4)(*ldir))
gl.glLightfv(gl.GL_LIGHT0, gl.GL_DIFFUSE,
(gl.GLfloat * 3)(1.0, 1.0, 1.0))
gl.glLightfv(gl.GL_LIGHT0, gl.GL_AMBIENT,
(gl.GLfloat * 3)(0.75, 0.75, 0.75))
gl.glEnable(gl.GL_LIGHT0)
gl.glEnable(gl.GL_NORMALIZE)
gl.glEnable(gl.GL_LIGHTING)
gl.glColor3f(1, 1, 1)
texture = image_data.get_texture()
gl.glEnable(texture.target)
gl.glBindTexture(texture.target, texture.id)
gl.glTexParameteri(
gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_NEAREST)
# comment this to get round points with MSAA on
gl.glEnable(gl.GL_POINT_SPRITE)
if not state.scale and not state.attenuation:
gl.glDisable(gl.GL_MULTISAMPLE) # for true 1px points with MSAA on
vertex_list.draw(gl.GL_POINTS)
gl.glDisable(texture.target)
if not state.scale and not state.attenuation:
gl.glEnable(gl.GL_MULTISAMPLE)
gl.glDisable(gl.GL_LIGHTING)
gl.glColor3f(0.25, 0.25, 0.25)
frustum(depth_intrinsics)
axes()
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
gl.glOrtho(0, width, 0, height, -1, 1)
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadIdentity()
gl.glMatrixMode(gl.GL_TEXTURE)
gl.glLoadIdentity()
gl.glDisable(gl.GL_DEPTH_TEST)
fps_display.draw()
def run(dt):
global w, h
window.set_caption("RealSense (%dx%d) %dFPS (%.2fms) %s" %
(w, h, 0 if dt == 0 else 1.0 / dt, dt * 1000,
"PAUSED" if state.paused else ""))
if state.paused:
return
success, frames = pipeline.try_wait_for_frames(timeout_ms=0)
if not success:
return
depth_frame = frames.get_depth_frame()
other_frame = frames.first(other_stream).as_video_frame()
depth_frame = decimate.process(depth_frame)
if state.postprocessing:
for f in filters:
depth_frame = f.process(depth_frame)
# Grab new intrinsics (may be changed by decimation)
depth_intrinsics = rs.video_stream_profile(
depth_frame.profile).get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
color_image = np.asanyarray(other_frame.get_data())
colorized_depth = colorizer.colorize(depth_frame)
depth_colormap = np.asanyarray(colorized_depth.get_data())
if state.color:
mapped_frame, color_source = other_frame, color_image
else:
mapped_frame, color_source = colorized_depth, depth_colormap
points = pc.calculate(depth_frame)
pc.map_to(mapped_frame)
# handle color source or size change
fmt = convert_fmt(mapped_frame.profile.format())
global image_data
if (image_data.format, image_data.pitch) != (fmt, color_source.strides[0]):
empty = (gl.GLubyte * (w * h * 3))()
image_data = pyglet.image.ImageData(w, h, fmt, empty)
# copy image data to pyglet
image_data.set_data(fmt, color_source.strides[0], color_source.ctypes.data)
verts = np.asarray(points.get_vertices(2)).reshape(h, w, 3)
texcoords = np.asarray(points.get_texture_coordinates(2))
if len(vertex_list.vertices) != verts.size:
vertex_list.resize(verts.size // 3)
# need to reassign after resizing
vertex_list.vertices = verts.ravel()
vertex_list.tex_coords = texcoords.ravel()
# copy our data to pre-allocated buffers, this is faster than assigning...
# pyglet will take care of uploading to GPU
def copy(dst, src):
"""copy numpy array to pyglet array"""
# timeit was mostly inconclusive, favoring slice assignment for safety
np.array(dst, copy=False)[:] = src.ravel()
# ctypes.memmove(dst, src.ctypes.data, src.nbytes)
copy(vertex_list.vertices, verts)
copy(vertex_list.tex_coords, texcoords)
if state.lighting:
# compute normals
dy, dx = np.gradient(verts, axis=(0, 1))
n = np.cross(dx, dy)
# can use this, np.linalg.norm or similar to normalize, but OpenGL can do this for us, see GL_NORMALIZE above
# norm = np.sqrt((n*n).sum(axis=2, keepdims=True))
# np.divide(n, norm, out=n, where=norm != 0)
# import cv2
# n = cv2.bilateralFilter(n, 5, 1, 1)
copy(vertex_list.normals, n)
if keys[pyglet.window.key.E]:
points.export_to_ply('./out.ply', mapped_frame)
pyglet.clock.schedule(run)
try:
pyglet.app.run()
finally:
pipeline.stop()
原始程序有个bug
https://github.com/IntelRealSense/librealsense/issues/3887
加上
.as_video_frame()
接下来物体宽度:
官方文档:
https://github.com/IntelRealSense/librealsense/tree/master/examples/measure
大家提的问题:
https://github.com/IntelRealSense/librealsense/issues/1904
https://github.com/IntelRealSense/librealsense/issues/1231
https://github.com/IntelRealSense/librealsense/issues/1904
https://github.com/IntelRealSense/librealsense/issues/1413#
https://github.com/IntelRealSense/librealsense/issues/5135
https://github.com/IntelRealSense/librealsense/blob/master/examples/example.hpp
https://github.com/IntelRealSense/librealsense/tree/master/examples/measure
关于例子:机翻
// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <librealsense2/rsutil.h>
#include "example.hpp" // Include short list of convenience functions for rendering
// 包括用于渲染的便捷功能的简短列表
// This example will require several standard data-structures and algorithms:
#define _USE_MATH_DEFINES
#include <math.h>
#include <queue>
#include <unordered_set>
#include <map>
#include <thread>
#include <atomic>
#include <mutex>
using pixel = std::pair<int, int>;
// Distance 3D is used to calculate real 3D distance between two pixels
// 距离3D用于计算两个像素之间的真实3D距离
float dist_3d(const rs2::depth_frame& frame, pixel u, pixel v);
// Toggle helper class will be used to render the two buttons
// controlling the edges of our ruler
// Toggle helper类将用于渲染控制标尺边缘的两个按钮
struct toggle
{
toggle() : x(0.f), y(0.f) {}
toggle(float xl, float yl)
: x(std::min(std::max(xl, 0.f), 1.f)),
y(std::min(std::max(yl, 0.f), 1.f))
{}
// Move from [0,1] space to pixel space of specific frame
// 从[0,1]空间移至特定帧的像素空间
pixel get_pixel(rs2::depth_frame frm) const
{
int px = x * frm.get_width();
int py = y * frm.get_height();
return{ px, py };
}
void render(const window& app)
{
glColor4f(0.f, 0.0f, 0.0f, 0.2f);
render_circle(app, 10);
render_circle(app, 8);
glColor4f(1.f, 0.9f, 1.0f, 1.f);
render_circle(app, 6);
}
void render_circle(const window& app, float r)
{
const float segments = 16;
glBegin(GL_TRIANGLE_STRIP);
for (auto i = 0; i <= segments; i++)
{
auto t = 2 * M_PI * float(i) / segments;
glVertex2f(x * app.width() + cos(t) * r,
y * app.height() + sin(t) * r);
glVertex2f(x * app.width(),
y * app.height());
}
glEnd();
}
// This helper function is used to find the button
// closest to the mouse cursor
// Since we are only comparing this distance, sqrt can be safely skipped
// 此辅助函数用于查找最接近鼠标光标的按钮
// 由于我们仅比较此距离,因此可以安全地跳过sqrt
float dist_2d(const toggle& other) const
{
return pow(x - other.x, 2) + pow(y - other.y, 2);
}
float x;
float y;
bool selected = false;
};
// Application state shared between the main-thread and GLFW events
// 主线程和GLFW事件之间共享的应用程序状态
struct state
{
bool mouse_down = false;
toggle ruler_start;
toggle ruler_end;
};
// Helper function to register to UI events
// 帮助程序功能注册到UI事件
void register_glfw_callbacks(window& app, state& app_state);
// Distance rendering functions:
// 距离渲染功能:
// Simple distance is the classic pythagorean distance between 3D points
// This distance ignores the topology of the object and can cut both through
// air and through solid
// 简单距离是3D点之间的经典勾股距离
// 这个距离忽略了物体的拓扑结构,可以穿过空气和固体
void render_simple_distance(const rs2::depth_frame& depth,
const state& s,
const window& app);
int main(int argc, char * argv[]) try
{
// OpenGL textures for the color and depth frames
// 彩色和深度帧的OpenGL纹理
texture depth_image, color_image;
// Colorizer is used to visualize depth data
// 着色器用于可视化深度数据
rs2::colorizer color_map;
// Use black to white color map
// 使用黑色到白色的颜色图
color_map.set_option(RS2_OPTION_COLOR_SCHEME, 2.f);
// Decimation filter reduces the amount of data (while preserving best samples)
// 抽取滤波器可减少数据量(同时保留最佳样本)
rs2::decimation_filter dec;
// If the demo is too slow, make sure you run in Release (-DCMAKE_BUILD_TYPE=Release)
// 如果速度太慢,请确保在Release (-DCMAKE_BUILD_TYPE=Release)中运行
// but you can also increase the following parameter to decimate depth more (reducing quality)
// 但您也可以增加以下参数来进一步降低深度(降低质量)
dec.set_option(RS2_OPTION_FILTER_MAGNITUDE, 2);
// Define transformations from and to Disparity domain
// 定义视差域之间的转换
rs2::disparity_transform depth2disparity;
rs2::disparity_transform disparity2depth(false);
// Define spatial filter (edge-preserving)
// 定义空间过滤器(边缘保留)
rs2::spatial_filter spat;
// Enable hole-filling
// Hole filling is an agressive heuristic and it gets the depth wrong many times
// However, this demo is not built to handle holes
// (the shortest-path will always prefer to "cut" through the holes since they have zero 3D distance)
// 启用孔填充
// 孔填充是一种攻击性的启发式方法,它多次导致深度错误
// 但是,此演示不是为处理孔洞而构建的
//(最短路径始终喜欢通过孔洞“切”,因为它们的零3D距离为零)
spat.set_option(RS2_OPTION_HOLES_FILL, 5); // 5 = fill all the zero pixels
// Define temporal filter
// 定义时间过滤器
rs2::temporal_filter temp;
// Spatially align all streams to depth viewport
// We do this because:
// a. Usually depth has wider FOV, and we only really need depth for this demo
// b. We don't want to introduce new holes
// 在空间上将所有流与深度视口对齐
// 我们这样做是因为:
// 通常,深度具有较宽的FOV,我们只需要此演示的深度
// 我们不想引入新的孔洞
rs2::align align_to(RS2_STREAM_DEPTH);
// Declare RealSense pipeline, encapsulating the actual device and sensors
// 声明RealSense管道,封装实际的设备和传感器
rs2::pipeline pipe;
rs2::config cfg;
cfg.enable_stream(RS2_STREAM_DEPTH); // Enable default depth //启用默认深度
// For the color stream, set format to RGBA
// To allow blending of the color frame on top of the depth frame
// 对于颜色流,将格式设置为RGBA
// 允许在深度框上方混合颜色框
cfg.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_RGBA8);
auto profile = pipe.start(cfg);
auto sensor = profile.get_device().first<rs2::depth_sensor>();
// Set the device to High Accuracy preset of the D400 stereoscopic cameras
// 将设备设置为D400立体摄像机的“高精度”预设
if (sensor && sensor.is<rs2::depth_stereo_sensor>())
{
sensor.set_option(RS2_OPTION_VISUAL_PRESET, RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY);
}
auto stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
// Create a simple OpenGL window for rendering:
// 创建一个简单的OpenGL窗口进行渲染:
window app(stream.width(), stream.height(), "RealSense Measure Example");
// Define application state and position the ruler buttons
// 定义应用程序状态并定位标尺按钮
state app_state;
app_state.ruler_start = { 0.45f, 0.5f };
app_state.ruler_end = { 0.55f, 0.5f };
register_glfw_callbacks(app, app_state);
// After initial post-processing, frames will flow into this queue:
// 初始后处理后,帧将流入此队列:
rs2::frame_queue postprocessed_frames;
// Alive boolean will signal the worker threads to finish-up
// 激活的布尔值将指示工作线程完成
std::atomic_bool alive{ true };
// Video-processing thread will fetch frames from the camera,
// apply post-processing and send the result to the main thread for rendering
// It recieves synchronized (but not spatially aligned) pairs
// and outputs synchronized and aligned pairs
// 视频处理线程将从摄像头获取帧,进行后处理并将结果发送到主线程进行渲染
// 接收同步(但在空间上不对齐)对,并输出同步和对齐对
std::thread video_processing_thread([&]() {
while (alive)
{
// Fetch frames from the pipeline and send them for processing
// 从管道中获取帧并将其发送以进行处理
rs2::frameset data;
if (pipe.poll_for_frames(&data))
{
// First make the frames spatially aligned
// 首先使框架在空间上对齐
data = data.apply_filter(align_to);
// Decimation will reduce the resultion of the depth image,
// closing small holes and speeding-up the algorithm
// 抽取会降低深度图像的分辨率,关闭小孔并加快算法
data = data.apply_filter(dec);
// To make sure far-away objects are filtered proportionally
// we try to switch to disparity domain
// 确保远处的对象按比例过滤
// 我们尝试切换到视差域
data = data.apply_filter(depth2disparity);
// Apply spatial filtering
// 应用空间过滤
data = data.apply_filter(spat);
// Apply temporal filtering
// 应用时间过滤
data = data.apply_filter(temp);
// If we are in disparity domain, switch back to depth
// 如果我们处于视差域,请切换回深度
data = data.apply_filter(disparity2depth);
Apply color map for visualization of depth
// 应用颜色图以可视化深度
data = data.apply_filter(color_map);
// Send resulting frames for visualization in the main thread
// 发送结果帧以在主线程中进行可视化
postprocessed_frames.enqueue(data);
}
}
});
rs2::frameset current_frameset;
while(app) // Application still alive?//应用程序还在激活状态吗
{
// Fetch the latest available post-processed frameset
// 获取最新的可用后处理框架集
postprocessed_frames.poll_for_frame(¤t_frameset);
if (current_frameset)
{
auto depth = current_frameset.get_depth_frame();
auto color = current_frameset.get_color_frame();
auto colorized_depth = current_frameset.first(RS2_STREAM_DEPTH, RS2_FORMAT_RGB8);
glEnable(GL_BLEND);
// Use the Alpha channel for blending
// 使用Alpha通道进行混合
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// First render the colorized depth image
// 首先渲染彩色深度图像
depth_image.render(colorized_depth, { 0, 0, app.width(), app.height() });
// Render the color frame (since we have selected RGBA format
// pixels out of FOV will appear transparent)
// 渲染颜色框(因为我们从FOV中选择了RGBA格式的像素将显示为透明)
color_image.render(color, { 0, 0, app.width(), app.height() });
// Render the simple pythagorean distance
// 渲染简单的勾股距离
render_simple_distance(depth, app_state, app);
// Render the ruler
// 渲染标尺
app_state.ruler_start.render(app);
app_state.ruler_end.render(app);
glColor3f(1.f, 1.f, 1.f);
glDisable(GL_BLEND);
}
}
// Signal threads to finish and wait until they do
// 通知线程完成并等待它们完成
alive = false;
video_processing_thread.join();
return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
return EXIT_FAILURE;
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
float dist_3d(const rs2::depth_frame& frame, pixel u, pixel v)
{
float upixel[2]; // From pixel像素
float upoint[3]; // From point (in 3D)
float vpixel[2]; // To pixel
float vpoint[3]; // To point (in 3D)
// Copy pixels into the arrays (to match rsutil signatures)
// 将像素复制到数组中(以匹配rsutil签名)
upixel[0] = u.first;
upixel[1] = u.second;
vpixel[0] = v.first;
vpixel[1] = v.second;
// Query the frame for distance
// Note: this can be optimized
// It is not recommended to issue an API call for each pixel
// (since the compiler can't inline these)
// However, in this example it is not one of the bottlenecks
// 查询框架的距离
// 注意:可以优化
// 不建议为每个像素发出API调用
//(因为编译器无法内联这些代码)
// 但是,在此示例中,这不是瓶颈之一
auto udist = frame.get_distance(upixel[0], upixel[1]);
auto vdist = frame.get_distance(vpixel[0], vpixel[1]);
// Deproject from pixel to point in 3D
// 在3D模式下从像素投影到点
rs2_intrinsics intr = frame.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data
rs2_deproject_pixel_to_point(upoint, &intr, upixel, udist);
rs2_deproject_pixel_to_point(vpoint, &intr, vpixel, vdist);
// Calculate euclidean distance between the two points
// 计算两点之间的欧几里得距离
return sqrt(pow(upoint[0] - vpoint[0], 2) +
pow(upoint[1] - vpoint[1], 2) +
pow(upoint[2] - vpoint[2], 2));
}
void draw_line(float x0, float y0, float x1, float y1, int width)
{
glPushAttrib(GL_ENABLE_BIT);
glLineStipple(1, 0x00ff);
glEnable(GL_LINE_STIPPLE);
glLineWidth(width);
glBegin(GL_LINE_STRIP);
glVertex2f(x0, y0);
glVertex2f(x1, y1);
glEnd();
glPopAttrib();
}
void render_simple_distance(const rs2::depth_frame& depth,
const state& s,
const window& app)
{
pixel center;
glColor4f(0.f, 0.0f, 0.0f, 0.2f);
draw_line(s.ruler_start.x * app.width(),
s.ruler_start.y * app.height(),
s.ruler_end.x * app.width(),
s.ruler_end.y * app.height(), 9);
glColor4f(0.f, 0.0f, 0.0f, 0.3f);
draw_line(s.ruler_start.x * app.width(),
s.ruler_start.y * app.height(),
s.ruler_end.x * app.width(),
s.ruler_end.y * app.height(), 7);
glColor4f(1.f, 1.0f, 1.0f, 1.f);
draw_line(s.ruler_start.x * app.width(),
s.ruler_start.y * app.height(),
s.ruler_end.x * app.width(),
s.ruler_end.y * app.height(), 3);
auto from_pixel = s.ruler_start.get_pixel(depth);
auto to_pixel = s.ruler_end.get_pixel(depth);
float air_dist = dist_3d(depth, from_pixel, to_pixel);
center.first = (from_pixel.first + to_pixel.first) / 2;
center.second = (from_pixel.second + to_pixel.second) / 2;
std::stringstream ss;
ss << int(air_dist * 100) << " cm";
auto str = ss.str();
auto x = (float(center.first) / depth.get_width()) * app.width() + 15;
auto y = (float(center.second) / depth.get_height()) * app.height() + 15;
auto w = stb_easy_font_width((char*)str.c_str());
// Draw dark background for the text label
// 为文字标签绘制深色背景
glColor4f(0.f, 0.f, 0.f, 0.4f);
glBegin(GL_TRIANGLES);
glVertex2f(x - 3, y - 10);
glVertex2f(x + w + 2, y - 10);
glVertex2f(x + w + 2, y + 2);
glVertex2f(x + w + 2, y + 2);
glVertex2f(x - 3, y + 2);
glVertex2f(x - 3, y - 10);
glEnd();
// Draw white text label
// 绘制白色文字标签
glColor4f(1.f, 1.f, 1.f, 1.f);
draw_text(x, y, str.c_str());
}
// Implement drag&drop behaviour for the buttons:
// 实现按钮的拖放行为:
void register_glfw_callbacks(window& app, state& app_state)
{
app.on_left_mouse = [&](bool pressed)
{
app_state.mouse_down = pressed;
};
app.on_mouse_move = [&](double x, double y)
{
toggle cursor{ float(x) / app.width(), float(y) / app.height() };
std::vector<toggle*> toggles{
&app_state.ruler_start,
&app_state.ruler_end };
if (app_state.mouse_down)
{
toggle* best = toggles.front();
for (auto&& t : toggles)
{
if (t->dist_2d(cursor) < best->dist_2d(cursor))
{
best = t;
}
}
best->selected = true;
}
else
{
for (auto&& t : toggles) t->selected = false;
}
for (auto&& t : toggles)
{
if (t->selected) *t = cursor;
}
};
}
计算宽度:
import pyrealsense2 as rs
import numpy as np
import cv2
import os
# opencv-haar人脸检测
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# Start streaming
pipe_profile = pipeline.start(config)
curr_frame = 0
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if frames.size() < 2:
continue
if not depth_frame or not color_frame:
continue
# Intrinsics & Extrinsics
# 深度相机内参矩阵
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
# RGB相机内参矩阵
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
# 深度图到彩图的外参RT
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(
color_frame.profile)
# print(depth_intrin.ppx, depth_intrin.ppy)
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# 将RGB对齐到深度,获取对应下的XYZ
#Color->Depth
align = rs.align(rs.stream.depth)
frameset = align.process(frames)
if frameset.size() < 2:
continue
depth_frame = frameset.get_depth_frame()
color_frame = frameset.get_color_frame()
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# 找到人脸
# find the human face in the color_image
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
left = []
for (x, y, w, h) in faces:
# 当前帧大于100
if curr_frame > 100 and curr_frame % 40 == 10:
# 取出人脸的深度图和彩色图
roi_depth_image = depth_image[y:y+h, x:x+w]
roi_color_image = color_image[y:y+h, x:x+w]
# 新建
os.system('mkdir -p ./3d_output/%d' % curr_frame)
# 保存
cv2.imwrite('./3d_output/%d/depth.jpg' %
curr_frame, roi_depth_image)
cv2.imwrite('./3d_output/%d/color.jpg' %
curr_frame, roi_color_image)
# write the depth data in a depth.txt
with open('./3d_output/%d/depth.csv' % curr_frame, 'w') as f:
# W
cols = list(range(x, x+w))
# H
rows = list(range(y, y+h))
for i in rows: #H
for j in cols: #W
# 坐标变换一定要注意检查
# 此时获取的是真实世界坐标的深度
# https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/hpp/rs_frame.hpp#L810
depth = depth_frame.get_distance(j, i) # W,H
# 给定没有失真或反失真系数的图像中的像素坐标和深度,计算相对于同一相机的3D空间中的对应点
# https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/rsutil.h#L67
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, [j, i], depth)
text = "%.5lf, %.5lf, %.5lf\n" % (
depth_point[0], depth_point[1], depth_point[2])
f.write(text)
if i==rows[0]:
left.append(depth_point)
print("Finish writing the depth img")
temp = np.array(left)
index = np.where(temp != 0)[0]
#dist2 = np.sqrt(np.square(left[index[-1]][0] - left[index[0]][0])+np.square(left[index[-1]][1] - left[index[0]][1])+np.square(left[index[-1]][2] - left[index[0]][2]))
# // 计算两点之间的欧几里得距离
# return sqrt(pow(upoint[0] - vpoint[0], 2) +
# pow(upoint[1] - vpoint[1], 2) +
# pow(upoint[2] - vpoint[2], 2));
#这里的距离,收到环境的影响,因为我是直接计算框里面最左端到最右端的距离
#如果把背景框进来,那么你测的是两个背景的宽度
print("dist","---------------------", str(left[index[-1]][0] - left[index[0]][0]))
cv2.putText(color_image, str(left[index[-1]][0] - left[index[0]][0]),
(x, y - 30),
cv2.FONT_HERSHEY_COMPLEX, 1.5, (255, 255, 255))
cv2.rectangle(color_image, (x, y), (x+w, y+h), (255, 0, 0), 2)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(
depth_image, alpha=0.03), cv2.COLORMAP_JET)
# Stack both images horizontally
images = np.hstack((color_image, depth_colormap))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
cv2.waitKey(1)
curr_frame += 1
finally:
# Stop streaming
pipeline.stop()
有一个显示相机位置的py例子:
# License: Apache 2.0. See LICENSE file in root directory.
# Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
"""
OpenGL Pointcloud viewer with http://pyglet.org
Usage:
------
Mouse:
Drag with left button to rotate around pivot (thick small axes),
with right button to translate and the wheel to zoom.
Keyboard:
[p] Pause
[r] Reset View
[d] Cycle through decimation values
[z] Toggle point scaling
[x] Toggle point distance attenuation
[c] Toggle color source
[l] Toggle lighting
[f] Toggle depth post-processing
[s] Save PNG (./out.png)
[e] Export points to ply (./out.ply)
[q/ESC] Quit
Notes:
------
Using deprecated OpenGL (FFP lighting, matrix stack...) however, draw calls
are kept low with pyglet.graphics.* which uses glDrawArrays internally.
Normals calculation is done with numpy on CPU which is rather slow, should really
be done with shaders but was omitted for several reasons - brevity, for lowering
dependencies (pyglet doesn't ship with shader support & recommends pyshaders)
and for reference.
"""
import math
import ctypes
import pyglet
import pyglet.gl as gl
import numpy as np
import pyrealsense2 as rs
# https://stackoverflow.com/a/6802723
def rotation_matrix(axis, theta):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
axis = axis / math.sqrt(np.dot(axis, axis))
a = math.cos(theta / 2.0)
b, c, d = -axis * math.sin(theta / 2.0)
aa, bb, cc, dd = a * a, b * b, c * c, d * d
bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
[2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
[2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])
class AppState:
def __init__(self, *args, **kwargs):
self.pitch, self.yaw = math.radians(-10), math.radians(-15)
self.translation = np.array([0, 0, 1], np.float32)
self.distance = 2
self.mouse_btns = [False, False, False]
self.paused = False
self.decimate = 0
self.scale = True
self.attenuation = False
self.color = True
self.lighting = False
self.postprocessing = False
def reset(self):
self.pitch, self.yaw, self.distance = 0, 0, 2
self.translation[:] = 0, 0, 1
@property
def rotation(self):
Rx = rotation_matrix((1, 0, 0), math.radians(-self.pitch))
Ry = rotation_matrix((0, 1, 0), math.radians(-self.yaw))
return np.dot(Ry, Rx).astype(np.float32)
state = AppState()
# Configure streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
# other_stream, other_format = rs.stream.infrared, rs.format.y8
other_stream, other_format = rs.stream.color, rs.format.rgb8
config.enable_stream(other_stream, 640, 480, other_format, 30)
# Start streaming
pipeline.start(config)
profile = pipeline.get_active_profile()
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
depth_intrinsics = depth_profile.get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
# Processing blocks
pc = rs.pointcloud()
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
colorizer = rs.colorizer()
filters = [rs.disparity_transform(),
rs.spatial_filter(),
rs.temporal_filter(),
rs.disparity_transform(False)]
# pyglet
window = pyglet.window.Window(
config=gl.Config(
double_buffer=True,
samples=8 # MSAA
),
resizable=True, vsync=True)
keys = pyglet.window.key.KeyStateHandler()
window.push_handlers(keys)
def convert_fmt(fmt):
"""rs.format to pyglet format string"""
return {
rs.format.rgb8: 'RGB',
rs.format.bgr8: 'BGR',
rs.format.rgba8: 'RGBA',
rs.format.bgra8: 'BGRA',
rs.format.y8: 'L',
}[fmt]
# Create a VertexList to hold pointcloud data
# Will pre-allocates memory according to the attributes below
vertex_list = pyglet.graphics.vertex_list(
w * h, 'v3f/stream', 't2f/stream', 'n3f/stream')
# Create and allocate memory for our color data
other_profile = rs.video_stream_profile(profile.get_stream(other_stream))
image_data = pyglet.image.ImageData(w, h, convert_fmt(
other_profile.format()), (gl.GLubyte * (w * h * 3))())
fps_display = pyglet.clock.ClockDisplay()
@window.event
def on_mouse_drag(x, y, dx, dy, buttons, modifiers):
w, h = map(float, window.get_size())
if buttons & pyglet.window.mouse.LEFT:
state.yaw -= dx * 0.5
state.pitch -= dy * 0.5
if buttons & pyglet.window.mouse.RIGHT:
dp = np.array((dx / w, -dy / h, 0), np.float32)
state.translation += np.dot(state.rotation, dp)
if buttons & pyglet.window.mouse.MIDDLE:
dz = dy * 0.01
state.translation -= (0, 0, dz)
state.distance -= dz
def handle_mouse_btns(x, y, button, modifiers):
state.mouse_btns[0] ^= (button & pyglet.window.mouse.LEFT)
state.mouse_btns[1] ^= (button & pyglet.window.mouse.RIGHT)
state.mouse_btns[2] ^= (button & pyglet.window.mouse.MIDDLE)
window.on_mouse_press = window.on_mouse_release = handle_mouse_btns
@window.event
def on_mouse_scroll(x, y, scroll_x, scroll_y):
dz = scroll_y * 0.1
state.translation -= (0, 0, dz)
state.distance -= dz
def on_key_press(symbol, modifiers):
if symbol == pyglet.window.key.R:
state.reset()
if symbol == pyglet.window.key.P:
state.paused ^= True
if symbol == pyglet.window.key.D:
state.decimate = (state.decimate + 1) % 3
decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
if symbol == pyglet.window.key.C:
state.color ^= True
if symbol == pyglet.window.key.Z:
state.scale ^= True
if symbol == pyglet.window.key.X:
state.attenuation ^= True
if symbol == pyglet.window.key.L:
state.lighting ^= True
if symbol == pyglet.window.key.F:
state.postprocessing ^= True
if symbol == pyglet.window.key.S:
pyglet.image.get_buffer_manager().get_color_buffer().save('out.png')
if symbol == pyglet.window.key.Q:
window.close()
window.push_handlers(on_key_press)
def axes(size=1, width=1):
"""draw 3d axes"""
gl.glLineWidth(width)
pyglet.graphics.draw(6, gl.GL_LINES,
('v3f', (0, 0, 0, size, 0, 0,
0, 0, 0, 0, size, 0,
0, 0, 0, 0, 0, size)),
('c3f', (1, 0, 0, 1, 0, 0,
0, 1, 0, 0, 1, 0,
0, 0, 1, 0, 0, 1,
))
)
def frustum(intrinsics):
"""draw camera's frustum"""
w, h = intrinsics.width, intrinsics.height
batch = pyglet.graphics.Batch()
for d in range(1, 6, 2):
def get_point(x, y):
p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d)
batch.add(2, gl.GL_LINES, None, ('v3f', [0, 0, 0] + p))
return p
top_left = get_point(0, 0)
top_right = get_point(w, 0)
bottom_right = get_point(w, h)
bottom_left = get_point(0, h)
batch.add(2, gl.GL_LINES, None, ('v3f', top_left + top_right))
batch.add(2, gl.GL_LINES, None, ('v3f', top_right + bottom_right))
batch.add(2, gl.GL_LINES, None, ('v3f', bottom_right + bottom_left))
batch.add(2, gl.GL_LINES, None, ('v3f', bottom_left + top_left))
batch.draw()
def grid(size=1, n=10, width=1):
"""draw a grid on xz plane"""
gl.glLineWidth(width)
s = size / float(n)
s2 = 0.5 * size
batch = pyglet.graphics.Batch()
for i in range(0, n + 1):
x = -s2 + i * s
batch.add(2, gl.GL_LINES, None, ('v3f', (x, 0, -s2, x, 0, s2)))
for i in range(0, n + 1):
z = -s2 + i * s
batch.add(2, gl.GL_LINES, None, ('v3f', (-s2, 0, z, s2, 0, z)))
batch.draw()
@window.event
def on_draw():
window.clear()
gl.glEnable(gl.GL_DEPTH_TEST)
gl.glEnable(gl.GL_LINE_SMOOTH)
width, height = window.get_size()
gl.glViewport(0, 0, width, height)
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
gl.gluPerspective(60, width / float(height), 0.01, 20)
gl.glMatrixMode(gl.GL_TEXTURE)
gl.glLoadIdentity()
# texcoords are [0..1] and relative to top-left pixel corner, add 0.5 to center
gl.glTranslatef(0.5 / image_data.width, 0.5 / image_data.height, 0)
# texture size may be increased by pyglet to a power of 2
tw, th = image_data.texture.owner.width, image_data.texture.owner.height
gl.glScalef(image_data.width / float(tw),
image_data.height / float(th), 1)
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadIdentity()
gl.gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0)
gl.glTranslatef(0, 0, state.distance)
gl.glRotated(state.pitch, 1, 0, 0)
gl.glRotated(state.yaw, 0, 1, 0)
if any(state.mouse_btns):
axes(0.1, 4)
gl.glTranslatef(0, 0, -state.distance)
gl.glTranslatef(*state.translation)
gl.glColor3f(0.5, 0.5, 0.5)
gl.glPushMatrix()
gl.glTranslatef(0, 0.5, 0.5)
grid()
gl.glPopMatrix()
psz = max(window.get_size()) / float(max(w, h)) if state.scale else 1
gl.glPointSize(psz)
distance = (0, 0, 1) if state.attenuation else (1, 0, 0)
gl.glPointParameterfv(gl.GL_POINT_DISTANCE_ATTENUATION,
(gl.GLfloat * 3)(*distance))
if state.lighting:
ldir = [0.5, 0.5, 0.5] # world-space lighting
ldir = np.dot(state.rotation, (0, 0, 1)) # MeshLab style lighting
ldir = list(ldir) + [0] # w=0, directional light
gl.glLightfv(gl.GL_LIGHT0, gl.GL_POSITION, (gl.GLfloat * 4)(*ldir))
gl.glLightfv(gl.GL_LIGHT0, gl.GL_DIFFUSE,
(gl.GLfloat * 3)(1.0, 1.0, 1.0))
gl.glLightfv(gl.GL_LIGHT0, gl.GL_AMBIENT,
(gl.GLfloat * 3)(0.75, 0.75, 0.75))
gl.glEnable(gl.GL_LIGHT0)
gl.glEnable(gl.GL_NORMALIZE)
gl.glEnable(gl.GL_LIGHTING)
gl.glColor3f(1, 1, 1)
texture = image_data.get_texture()
gl.glEnable(texture.target)
gl.glBindTexture(texture.target, texture.id)
gl.glTexParameteri(
gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_NEAREST)
# comment this to get round points with MSAA on
gl.glEnable(gl.GL_POINT_SPRITE)
if not state.scale and not state.attenuation:
gl.glDisable(gl.GL_MULTISAMPLE) # for true 1px points with MSAA on
vertex_list.draw(gl.GL_POINTS)
gl.glDisable(texture.target)
if not state.scale and not state.attenuation:
gl.glEnable(gl.GL_MULTISAMPLE)
gl.glDisable(gl.GL_LIGHTING)
gl.glColor3f(0.25, 0.25, 0.25)
frustum(depth_intrinsics)
axes()
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
gl.glOrtho(0, width, 0, height, -1, 1)
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadIdentity()
gl.glMatrixMode(gl.GL_TEXTURE)
gl.glLoadIdentity()
gl.glDisable(gl.GL_DEPTH_TEST)
fps_display.draw()
def run(dt):
global w, h
window.set_caption("RealSense (%dx%d) %dFPS (%.2fms) %s" %
(w, h, 0 if dt == 0 else 1.0 / dt, dt * 1000,
"PAUSED" if state.paused else ""))
if state.paused:
return
success, frames = pipeline.try_wait_for_frames(timeout_ms=0)
if not success:
return
depth_frame = frames.get_depth_frame()
other_frame = frames.first(other_stream)
depth_frame = decimate.process(depth_frame)
if state.postprocessing:
for f in filters:
depth_frame = f.process(depth_frame)
# Grab new intrinsics (may be changed by decimation)
depth_intrinsics = rs.video_stream_profile(
depth_frame.profile).get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
color_image = np.asanyarray(other_frame.get_data())
colorized_depth = colorizer.colorize(depth_frame)
depth_colormap = np.asanyarray(colorized_depth.get_data())
if state.color:
mapped_frame, color_source = other_frame, color_image
else:
mapped_frame, color_source = colorized_depth, depth_colormap
points = pc.calculate(depth_frame)
pc.map_to(mapped_frame)
# handle color source or size change
fmt = convert_fmt(mapped_frame.profile.format())
global image_data
if (image_data.format, image_data.pitch) != (fmt, color_source.strides[0]):
empty = (gl.GLubyte * (w * h * 3))()
image_data = pyglet.image.ImageData(w, h, fmt, empty)
# copy image data to pyglet
image_data.set_data(fmt, color_source.strides[0], color_source.ctypes.data)
verts = np.asarray(points.get_vertices(2)).reshape(h, w, 3)
texcoords = np.asarray(points.get_texture_coordinates(2))
if len(vertex_list.vertices) != verts.size:
vertex_list.resize(verts.size // 3)
# need to reassign after resizing
vertex_list.vertices = verts.ravel()
vertex_list.tex_coords = texcoords.ravel()
# copy our data to pre-allocated buffers, this is faster than assigning...
# pyglet will take care of uploading to GPU
def copy(dst, src):
"""copy numpy array to pyglet array"""
# timeit was mostly inconclusive, favoring slice assignment for safety
np.array(dst, copy=False)[:] = src.ravel()
# ctypes.memmove(dst, src.ctypes.data, src.nbytes)
copy(vertex_list.vertices, verts)
copy(vertex_list.tex_coords, texcoords)
if state.lighting:
# compute normals
dy, dx = np.gradient(verts, axis=(0, 1))
n = np.cross(dx, dy)
# can use this, np.linalg.norm or similar to normalize, but OpenGL can do this for us, see GL_NORMALIZE above
# norm = np.sqrt((n*n).sum(axis=2, keepdims=True))
# np.divide(n, norm, out=n, where=norm != 0)
# import cv2
# n = cv2.bilateralFilter(n, 5, 1, 1)
copy(vertex_list.normals, n)
if keys[pyglet.window.key.E]:
points.export_to_ply('./out.ply', mapped_frame)
pyglet.clock.schedule(run)
try:
pyglet.app.run()
finally:
pipeline.stop()
结合改进:
import pyrealsense2 as rs
import numpy as np
import cv2
import os
# opencv-haar人脸检测
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# Start streaming
pipe_profile = pipeline.start(config)
curr_frame = 0
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if frames.size() < 2:
continue
if not depth_frame or not color_frame:
continue
# Intrinsics & Extrinsics
# 深度相机内参矩阵
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
# RGB相机内参矩阵
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
# 深度图到彩图的外参RT
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(
color_frame.profile)
depth_value = 0.5
depth_pixel = [depth_intrin.ppx, depth_intrin.ppy]
depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, depth_value)
print(depth_point)
# print(depth_intrin.ppx, depth_intrin.ppy)
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# 将RGB对齐到深度,获取对应下的XYZ
#Color->Depth
align = rs.align(rs.stream.depth)
frameset = align.process(frames)
if frameset.size() < 2:
continue
depth_frame = frameset.get_depth_frame()
color_frame = frameset.get_color_frame()
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# 找到人脸
# find the human face in the color_image
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
left = []
for (x, y, w, h) in faces:
# 当前帧大于100
if curr_frame > 100 and curr_frame % 40 == 10:
# 取出人脸的深度图和彩色图
roi_depth_image = depth_image[y:y+h, x:x+w]
roi_color_image = color_image[y:y+h, x:x+w]
# 新建
os.system('mkdir -p ./3d_output/%d' % curr_frame)
# 保存
cv2.imwrite('./3d_output/%d/depth.jpg' %
curr_frame, roi_depth_image)
cv2.imwrite('./3d_output/%d/color.jpg' %
curr_frame, roi_color_image)
# write the depth data in a depth.txt
with open('./3d_output/%d/depth.csv' % curr_frame, 'w') as f:
# W
cols = list(range(x, x+w))
# H
rows = list(range(y, y+h))
for i in rows: #H
for j in cols: #W
# 坐标变换一定要注意检查
# 此时获取的是真实世界坐标的深度
# https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/hpp/rs_frame.hpp#L810
depth = depth_frame.get_distance(j, i) # W,H
# 给定没有失真或反失真系数的图像中的像素坐标和深度,计算相对于同一相机的3D空间中的对应点
# https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/rsutil.h#L67
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, [j, i], depth)
text = "%.5lf, %.5lf, %.5lf\n" % (
depth_point[0], depth_point[1], depth_point[2])
f.write(text)
if i==rows[0]:
left.append(depth_point)
print("Finish writing the depth img")
temp = np.array(left)
index = np.where(temp != 0)[0]
#dist2 = np.sqrt(np.square(left[index[-1]][0] - left[index[0]][0])+np.square(left[index[-1]][1] - left[index[0]][1])+np.square(left[index[-1]][2] - left[index[0]][2]))
# // 计算两点之间的欧几里得距离
# return sqrt(pow(upoint[0] - vpoint[0], 2) +
# pow(upoint[1] - vpoint[1], 2) +
# pow(upoint[2] - vpoint[2], 2));
#这里的距离,收到环境的影响,因为我是直接计算框里面最左端到最右端的距离
#如果把背景框进来,那么你测的是两个背景的宽度
print("dist","---------------------", str(left[index[-1]][0] - left[index[0]][0]))
# 这里要做很多工作,离群噪声点的去除,去除后矩阵的真实大小判断 很多行,哪一行是最真实的距离
cv2.putText(color_image, str(left[index[-1]][0] - left[index[0]][0]),
(x, y - 30),
cv2.FONT_HERSHEY_COMPLEX, 1.5, (255, 255, 255))
cv2.rectangle(color_image, (x, y), (x+w, y+h), (255, 0, 0), 2)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(
depth_image, alpha=0.03), cv2.COLORMAP_JET)
# Stack both images horizontally
images = np.hstack((color_image, depth_colormap))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
cv2.waitKey(1)
curr_frame += 1
finally:
# Stop streaming
pipeline.stop()
修改版:
import pyrealsense2 as rs
import numpy as np
import cv2
import os
# opencv-haar人脸检测
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# Start streaming
pipe_profile = pipeline.start(config)
curr_frame = 0
try:
while True:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if frames.size() < 2:
continue
if not depth_frame or not color_frame:
continue
# Intrinsics & Extrinsics
# 深度相机内参矩阵
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
# RGB相机内参矩阵
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
# 深度图到彩图的外参RT
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(
color_frame.profile)
depth_value = 0.5
depth_pixel = [depth_intrin.ppx, depth_intrin.ppy]
depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, depth_value)
#print(depth_point)
# print(depth_intrin.ppx, depth_intrin.ppy)
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# 将RGB对齐到深度,获取对应下的XYZ
#Color->Depth
align = rs.align(rs.stream.depth)
frameset = align.process(frames)
if frameset.size() < 2:
continue
depth_frame = frameset.get_depth_frame()
color_frame = frameset.get_color_frame()
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# 找到人脸
# find the human face in the color_image
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
left = []
for (x, y, w, h) in faces:
# 当前帧大于100
if curr_frame > 100 and curr_frame % 40 == 10:
# 取出人脸的深度图和彩色图
roi_depth_image = depth_image[y:y+h, x:x+w]
roi_color_image = color_image[y:y+h, x:x+w]
# W
cols = list(range(x, x+w))
# H
rows = list(range(y, y+h))
for i in rows: #H
for j in cols: #W
# 坐标变换一定要注意检查
# 此时获取的是真实世界坐标的深度
# https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/hpp/rs_frame.hpp#L810
depth = depth_frame.get_distance(j, i) # W,H
# 给定没有失真或反失真系数的图像中的像素坐标和深度,计算相对于同一相机的3D空间中的对应点
# https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/rsutil.h#L67
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, [j, i], depth)
text = "%.5lf, %.5lf, %.5lf\n" % (
depth_point[0], depth_point[1], depth_point[2])
#f.write(text)
if i==rows[0]:
left.append(depth_point)
#print("Finish writing the depth img")
# temp = np.array(left)
# # 求均值
# _mean = np.mean(temp, axis=0)
# # 求方差
# _var = np.var(temp, axis=0)
# minmean = _mean - 1 * abs(_mean)
# maxmean = _mean + 1 * abs(_mean)
# minvar = _var - 1 * abs(_var)
# maxvar = _var + 1 * abs(_var)
def non_zero_mean(np_arr, axis):
exist = (np_arr != 0)
num = np_arr.sum(axis=axis)
den = exist.sum(axis=axis)
return num / den
temp = np.array(left)
# 求均值
_mean = non_zero_mean(temp, axis=0)
# 求方差
_var = np.var(temp, axis=0)
minmean = _mean - 1 * abs(_mean)
maxmean = _mean + 1 * abs(_mean)
minvar = _var - 1 * abs(_var)
maxvar = _var + 1 * abs(_var)
index = []
i = 0 # H
for j in range(len(cols)): # W
if temp[j][0] != 0 and temp[j][1] != 0 and temp[j][2] != 0:
if temp[j][0]>minmean[0] and temp[j][0]<maxmean[0]:
if temp[j][1] > minmean[1] and temp[j][1] < maxmean[1]:
if temp[j][2] > minmean[2] and temp[j][2] < maxmean[2]:
index.append(j)
#dist2 = np.sqrt(np.square(left[index[-1]][0] - left[index[0]][0])+np.square(left[index[-1]][1] - left[index[0]][1])+np.square(left[index[-1]][2] - left[index[0]][2]))
# // 计算两点之间的欧几里得距离
# return sqrt(pow(upoint[0] - vpoint[0], 2) +
# pow(upoint[1] - vpoint[1], 2) +
# pow(upoint[2] - vpoint[2], 2));
#这里的距离,收到环境的影响,因为我是直接计算框里面最左端到最右端的距离
#如果把背景框进来,那么你测的是两个背景的宽度
if len(index) > (len(cols)/2):
# 新建
os.system('mkdir -p ./3d_output/%d' % curr_frame)
# 保存
cv2.imwrite('./3d_output/%d/depth.jpg' %
curr_frame, roi_depth_image)
cv2.imwrite('./3d_output/%d/color.jpg' %
curr_frame, roi_color_image)
print("dist","---------------------", str(left[index[-1]][0] - left[index[0]][0]))
# 这里要做很多工作,离群噪声点的去除,去除后矩阵的真实大小判断 很多行,哪一行是最真实的距离
cv2.putText(color_image, str(left[index[-1]][0] - left[index[0]][0]),
(x, y - 30),
cv2.FONT_HERSHEY_COMPLEX, 1.5, (255, 255, 255))
cv2.rectangle(color_image, (x, y), (x+w, y+h), (255, 0, 0), 2)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(
depth_image, alpha=0.03), cv2.COLORMAP_JET)
# Stack both images horizontally
images = np.hstack((color_image, depth_colormap))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', images)
cv2.waitKey(1)
curr_frame += 1
finally:
# Stop streaming
pipeline.stop()
https://github.com/IntelRealSense/librealsense/issues/2351
https://github.com/IntelRealSense/librealsense/issues/2343
https://dev.intelrealsense.com/docs/rs-measure
https://intelrealsense.github.io/librealsense/python_docs/_generated/pyrealsense2.html
计算速度的例子
计算体积的例子
https://github.com/IntelRealSense/librealsense/issues/4612