SQ 小车避障 Intel Realsense D435 基于线性梯度的深度值过滤
2024-06-12 10:03:08
原理图
相关代码
# -*- coding: utf-8 -*-
"""
@File : 191224_obstacle_detection_建立梯度.py
@Time : 2019/12/24 14:51
@Author : Dontla
@Email : sxana@qq.com
@Software: PyCharm
"""import time
import numpy as np
import pyrealsense2 as rs
import cv2
import sys
from numba import jit# @jit
# 貌似开不了jit，不知啥原因，开了也没明显看到加速
def filter_alpha(depth_image, filter_alpha):if filter_alpha > 1:# 获取depth_image宽高h, w = depth_image.shape[0], depth_image.shape[1] # 360,640# 创建上下alpha（不同方法都能创建）# filter_upper = np.array([1] * int(h / 2))filter_upper = np.full(int(h / 2), 1)filter_lower = np.linspace(1, filter_alpha, h / 2)# 将filter_upper和filter_lower连在一起filter = np.r_[filter_upper, filter_lower]# print(filter)# print(filter.shape) # (360,)# print(filter_alpha_upper)# print(filter_alpha_upper.shape) # (180,)# print(filter_alpha_lower)# print(filter_alpha_lower.shape) # (180,)return (depth_image.T * filter).Telse:return depth_image# 如果要防止下面cotton过近被误探测,可用两层for循环设置梯度过滤
# 不过貌似还得中间对半分,下面直接舍弃掉,只用上面作为判断,因为就算下面用了梯度...(还是得用梯度...)
@jit
def traversing_pixels(depth_image, threshold_dangerous_distance):num_dangerous = 0num_all_pixels = 0depth_image_ravel = depth_image.ravel()# depth_image_segmentation为分割后的图像（红蓝两色）depth_image_segmentation_ravel = []for pixel in depth_image_ravel:num_all_pixels += 1# 第一种效果要好一些if pixel < threshold_dangerous_distance and pixel != 0:# if pixel < threshold_dangerous_distance:num_dangerous += 1depth_image_segmentation_ravel.append(0)else:depth_image_segmentation_ravel.append(6000)depth_image_segmentation = np.array(depth_image_segmentation_ravel).reshape(depth_image.shape)return num_all_pixels, num_dangerous, depth_image_segmentationclass ObstacleDetection(object):def __init__(self):# self.cam_serials = ['838212073161', '827312071726']# self.cam_serials = ['838212073161', '827312071726', '838212073249', '827312070790', '836612072369',# '826212070395']self.cam_serials = ['838212073161']self.cam_width, self.cam_height = 640, 360# 【危险距离：单位mm】self.threshold_dangerous_distance = 3000# 【摄像头到cotton平面垂直距离（单位mm）】self.distance_cam_vertical_to_cotton_top = 260# 【危险距离补偿系数】用于让最下面深度远离临界值，避免造成误检测self.factor_compensation_dangerous_distance = 1.5# 【危险距离像素占比】self.threshold_dangerous_scale = 0.05# 【摄像头视场角（单位°）】self.FOV_width = 69.4self.FOV_height = 42.5self.FOV_scale = self.FOV_height / self.FOV_width # 0.6123919308357348# 【实际变换后height视场角】if self.cam_height / self.cam_width < self.FOV_scale:self.FOV_height_actual = self.FOV_width * self.cam_height / self.cam_widthelse:self.FOV_height_actual = self.FOV_height# 【计算过滤α值（distance_min为图像最下方的深度，看到最近cotton的距离）】# 当摄像头到cotton顶垂直距离为800，最小距离为2256，当危险距离为2000时，alpha滤值为0.88# 当摄像头到cotton顶垂直距离为800，最小距离为2256，当危险距离为3000时，alpha滤值为1.32# 所以，后面进行滤值时需判断self.filter_alpha的值是否大于1（已添加进filter_alpha()函数中）self.distance_min = self.distance_cam_vertical_to_cotton_top / (np.tan(self.FOV_height_actual / 2 * np.pi / 180))self.filter_alpha = self.threshold_dangerous_distance / self.distance_min * self.factor_compensation_dangerous_distancedef obstacle_detection(self):# print(self.distance_min) # 2256.7829632201597# print(self.filter_alpha) # 0.8862172537611853# 摄像头个数（在这里设置所需使用摄像头的总个数）cam_num = 6ctx = rs.context()'''连续验证机制'''# D·C 1911202：创建最大验证次数max_veri_times；创建连续稳定值continuous_stable_value，用于判断设备重置后是否处于稳定状态max_veri_times = 100continuous_stable_value = 5print('\n', end='')print('开始连续验证，连续验证稳定值：{}，最大验证次数：{}：'.format(continuous_stable_value, max_veri_times))continuous_value = 0veri_times = 0while True:devices = ctx.query_devices()connected_cam_num = len(devices)print('摄像头个数：{}'.format(connected_cam_num))if connected_cam_num == cam_num:continuous_value += 1if continuous_value == continuous_stable_value:breakelse:continuous_value = 0veri_times += 1if veri_times == max_veri_times:print("检测超时，请检查摄像头连接！")sys.exit()'''循环reset摄像头'''# hardware_reset()后是不是应该延迟一段时间？不延迟就会报错print('\n', end='')print('开始初始化摄像头：')for dev in ctx.query_devices():# 先将设备的序列号放进一个变量里，免得在下面for循环里访问设备的信息过多（虽然不知道它会不会每次都重新访问）dev_serial = dev.get_info(rs.camera_info.serial_number)# 匹配序列号，重置我们需重置的特定摄像头（注意两个for循环顺序，哪个在外哪个在内很重要，不然会导致刚重置的摄像头又被访问导致报错）for serial in self.cam_serials:if serial == dev_serial:dev.hardware_reset()# 像下面这条语句居然不会报错，不是刚刚才重置了dev吗？莫非区别在于没有通过for循环ctx.query_devices()去访问？# 是不是刚重置后可以通过ctx.query_devices()去查看有这个设备，但是却没有存储设备地址？如果是这样，# 也就能够解释为啥能够通过len(ctx.query_devices())函数获取设备数量，但访问序列号等信息就会报错的原因了print('摄像头{}初始化成功'.format(dev.get_info(rs.camera_info.serial_number)))'''连续验证机制'''# D·C 1911202：创建最大验证次数max_veri_times；创建连续稳定值continuous_stable_value，用于判断设备重置后是否处于稳定状态print('\n', end='')print('开始连续验证，连续验证稳定值：{}，最大验证次数：{}：'.format(continuous_stable_value, max_veri_times))continuous_value = 0veri_times = 0while True:devices = ctx.query_devices()connected_cam_num = len(devices)print('摄像头个数：{}'.format(connected_cam_num))if connected_cam_num == cam_num:continuous_value += 1if continuous_value == continuous_stable_value:breakelse:continuous_value = 0veri_times += 1if veri_times == max_veri_times:print("检测超时，请检查摄像头连接！")sys.exit()'''配置各个摄像头的基本对象'''for i in range(len(self.cam_serials)):locals()['pipeline' + str(i)] = rs.pipeline(ctx)locals()['config' + str(i)] = rs.config()locals()['config' + str(i)].enable_device(self.cam_serials[i])# 为啥我设置成1280×720就报错呢？明明Intel Realsense的usb接口已经显示为3.0了# locals()['config' + str(i)].enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)# locals()['config' + str(i)].enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)locals()['config' + str(i)].enable_stream(rs.stream.depth, self.cam_width, self.cam_height, rs.format.z16,30)locals()['config' + str(i)].enable_stream(rs.stream.color, self.cam_width, self.cam_height, rs.format.bgr8,30)locals()['pipeline' + str(i)].start(locals()['config' + str(i)])# 创建对齐对象（深度对齐颜色）locals()['align' + str(i)] = rs.align(rs.stream.color)'''运行摄像头'''try:while True:start_time = time.time()for i in range(len(self.cam_serials)):locals()['frames' + str(i)] = locals()['pipeline' + str(i)].wait_for_frames()# 获取对齐帧集locals()['aligned_frames' + str(i)] = locals()['align' + str(i)].process(locals()['frames' + str(i)])# 获取对齐后的深度帧和彩色帧locals()['aligned_depth_frame' + str(i)] = locals()['aligned_frames' + str(i)].get_depth_frame()locals()['color_frame' + str(i)] = locals()['aligned_frames' + str(i)].get_color_frame()if not locals()['aligned_depth_frame' + str(i)] or not locals()['color_frame' + str(i)]:continue# 获取颜色帧内参locals()['color_profile' + str(i)] = locals()['color_frame' + str(i)].get_profile()locals()['cvsprofile' + str(i)] = rs.video_stream_profile(locals()['color_profile' + str(i)])locals()['color_intrin' + str(i)] = locals()['cvsprofile' + str(i)].get_intrinsics()locals()['color_intrin_part' + str(i)] = [locals()['color_intrin' + str(i)].ppx,locals()['color_intrin' + str(i)].ppy,locals()['color_intrin' + str(i)].fx,locals()['color_intrin' + str(i)].fy]locals()['color_image' + str(i)] = np.asanyarray(locals()['color_frame' + str(i)].get_data())locals()['depth_image' + str(i)] = np.asanyarray(locals()['aligned_depth_frame' + str(i)].get_data())# 【阿尔法过滤】locals()['depth_image_alpha_filter' + str(i)] = filter_alpha(locals()['depth_image' + str(i)],self.filter_alpha)# 【遍历深度图像素值,如存在小于危险值范围比例超过阈值,则告警】locals()['num_all_pixels' + str(i)], locals()['num_dangerous' + str(i)], locals()['depth_image_segmentation' + str(i)] = traversing_pixels(locals()['depth_image_alpha_filter' + str(i)], self.threshold_dangerous_distance)print('num_all_pixels:{}'.format(locals()['num_all_pixels' + str(i)]))print('num_dangerous:{}'.format(locals()['num_dangerous' + str(i)]))locals()['dangerous_scale' + str(i)] = locals()['num_dangerous' + str(i)] / locals()['num_all_pixels' + str(i)]print('危险比例:{}'.format(locals()['dangerous_scale' + str(i)]))if locals()['dangerous_scale' + str(i)] > self.threshold_dangerous_scale:print("距离警告!")locals()['depth_colormap' + str(i)] = cv2.applyColorMap(cv2.convertScaleAbs(locals()['depth_image_segmentation' + str(i)], alpha=0.0425),cv2.COLORMAP_JET)locals()['image' + str(i)] = np.hstack((locals()['color_image' + str(i)], locals()['depth_colormap' + str(i)]))# 注意: 窗口名不要用中文字符, 小心乱码cv2.imshow('win{}:{}'.format(i, self.cam_serials[i]), locals()['image' + str(i)])# cv2.imshow('colorWin{}: {}'.format(i, self.cam_serials[i]), locals()['color_image' + str(i)])# cv2.imshow('depthWin{}: {}'.format(i, self.cam_serials[i]), locals()['depth_colormap' + str(i)])cv2.waitKey(1)end_time = time.time()# print('单帧运行时间：{}'.format(end_time - start_time))# 遇到异常再次启动检测函数，如有需要可以将连续监测和摄像头重置全放进去except:print('\n出现异常，请重新检查摄像头连接！\n')for i in range(len(self.cam_serials)):cv2.destroyAllWindows()locals()['pipeline' + str(i)].stop()ObstacleDetection().obstacle_detection()finally:for i in range(len(self.cam_serials)):locals()['pipeline' + str(i)].stop()if __name__ == '__main__':ObstacleDetection().obstacle_detection()参考文章：为什么Intel Realsense D435深度摄像头在基于深度的水平方向障碍物检测（避障）方案中，摄像头不宜安装太高？
SQ 小车避障 Intel Realsense D435 基于线性梯度的深度值过滤相关推荐
- 为什么Intel Realsense D435深度摄像头在基于深度的水平方向障碍物检测（避障）方案中，摄像头不宜安装太高？
补一句,为啥R与b对比就要比R与a对比更容易区分? 因为a与b的右端和障碍物R属同一深度,但b左端明显比a左端深度大,总体比a拥有差异性更大的深度范围,因此b与a相比,对R拥有差异性更大的深度范围, ...
- 20200314 SQ Intel Realsense D435 USB 线长、转接线个数测试数据表
测试名称 Intel Realsense D435 摄像头USB线支持最大线长.转接线个数测试 测试参数 Platform:win10x64 系统内存:32G 测试程序:tensorflow_yolo ...
- python Intel Realsense D435 多线程资源分配问题（卡住、卡死）
在使用python多线程调用Intel Realsense D435多个摄像头时,发现pyrealsense的例如pipeline.start().context.query_devices()函数会 ...
- 激光与红外线的区别？激光雷达与毫米波雷达区别？毫米波雷达无法感知行人吗？关于Intel Realsense D435有源红外技术猜想
文章目录 激光与红外线的区别是什么呢? 激光雷达和毫米波雷达区别 毫米波雷达无法感知行人吗? 关于Intel Realsense D435有源红外技术猜想 阅读Intel Realsense文档的时候 ...
- WeMos下实现小车避障与手机控制
参考:WeMos下实现小车避障与手机控制 作者:爱学习的小王呀 发布时间:2020-10-02 10:17:45 网址:https://blog.csdn.net/hongliwong/article ...
- Kinect v2和Intel RealSense D435的三维重建对比
老师让我用Realsense和Kinect对金属物体进行三维重建,分析对比它们重建效果. 写下这个实验过程和结果记录一下. 实验设备:Kinect v2,Intel RealSense D435 软件 ...
- python如何拟合三维平面（拟合Intel Realsense D435深度数据点）
文章目录 拟合Intel Realsense D435深度数据点 参考文章:[MQ笔记]超简单的最小二乘法拟合平面(Python) import numpy as np import matplotl ...
- Tensorflow_yolov3 Intel Realsense D435奇怪的现象，多摄像头连接时一旦能检测到深度马上就会卡（卡住）
两个摄像头连接时一旦能检测到深度马上就会卡(小于30公分),,单个摄像头没事,这是使用了多线程传输后的现象,不知咋回事... 后来加了这句验证全局变量是否存在,好像好点了,有待验证 20200401 ...
- Intel Realsense D435 python multiprocessing 摄像头多进程流传输
参考文章1:python 测试multiprocessing多进程 参考文章2:Intel Realsense D435 多摄像头目标识别架构
最新文章
- LayoutParams cannot be resolved to a type
- 我做的4周岁儿童编程课程【视频】分享
- 后缀数组总结（转载）
- Excahnge 2010断开连接的邮箱无法找到
- vim学习笔记（3）眼花缭乱的Vim模式
- Github+docsify打造在线文档网站
- 前端路由的两种实现原理
- 《只管去做》-如何做靠谱的年度计划
- C语言计算机二级考动态分配内存吗,08年计算机二级C语言辅导：malloc动态分配的内存的生存周期是多少?...
- 我踩过的支付通道的“坑”（一）
- 河海大学计算机考研资料汇总
- 神经网络模拟的优缺点是,神经网络模型的优缺点
- WAPI协议完整性测试解析系列（之一）WAI子类型的协议完整性测试
- Element概述、入门案例及布局
- 电视机和计算机的显示器各使用什么扫描方式,扫描方式
- SpringCloud(若依微服务版为例)集成WebSocket实现前后端的消息推送
- kvm虚拟机压缩qcow2镜像空间
- one 主格 复数 宾格_主格与宾格.ppt
- #今日论文推荐# 斯坦福开发微型机器人，改善靶向给药技术
- vue-03-4：vue封装方法到工具类