原理圖

相關代碼

# -*- 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 基于线性梯度的深度值过滤的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。