intel realsenseD435Iで路面状況をセンシングしてみた！

#はじめに
　クルマイスの自動運転で必要な舗装路面から脱輪しないように、画像情報から方向修正を促す機能を作ってみた。
※今回、初投稿により見苦しい点があるかもしれないが、御容赦願いたい

#開発環境&使用機材
　開発環境: ・python3 　　・Anaconda Powershell Prompt (anaconda3)
使用機材:　intel realsenseD435i

#どうやって舗装路面と未舗装路面見分ける？
　舗装路面から脱輪しないようにするために、色相の変化に着目して画像センシングすることにした。具体的にはアスファルト＝黒、未舗装＝緑という基本パターンを見分けられるように画像を2値化して、その２値化画像を基に方向修正の判断を行う。

#プログラム
import pyrealsense2 as rs
import numpy as np
# Import OpenCV for easy image rendering
import cv2
pipeline = rs.pipeline()
config = rs.config()
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
found_rgb = False

for s in device.sensors:
    if s.get_info(rs.camera_info.name) == 'RGB Camera':
        found_rgb = True
        break

if not found_rgb:
    print("The demo requires Depth camera with Color sensor")
    exit(0)

config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
if device_product_line == 'L500':
    config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
else:
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()#深度スケールをメー トルで取得
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: " , depth_scale)
clipping_distance_in_meters = 0.25 #排除する距離パラメータ[m]
clipping_distance = clipping_distance_in_meters / depth_scale #depth基準作り
align = rs.align(align_to)
c=0
try:
    while True:
#データ取得
        frames = pipeline.wait_for_frames()
# frames.get_depth_frame() is a 640x360 depth image

# Align the depth frame to color frame
        aligned_frames = align.process(frames)

# Get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame() # Dデータ
        color_frame = aligned_frames.get_color_frame() #RGBデータ

# Validate that both frames are valid
        if not aligned_depth_frame or not color_frame:
            continue                                   
        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())#RGB表示3ch
            height, width = depth_image.shape
            #height, width, ch = color_image.shape
            #print(ch)
            depth_colormap = 
            cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.5), 
            cv2.COLORMAP_BONE)#α=透明度 COLORMAP_BONE
    
#画質変更
            h=320
            w=240
            dst = cv2.resize(color_image, dsize=(w, h))
            dst_depth = cv2.resize(depth_image, dsize=(w, h))
            depth_gry = cv2.resize(depth_colormap,dsize=(w, h))

# ぼかし加工。
            for i in range(4):
                average_square_size = 10  #ぼかしパラメータ 大きくする程にぼけていくdef=15
                sigma_color = 5000       # 色空間に関する標準偏差パラメータ  大きくすると色の平滑化範囲を広げるdef=5000
                sigma_metric = 1# 距離空間に関する標準偏差パラメータ  大きくすると色の平滑化範囲を広げる (d==0の時のみ有効)
                img_bilateral = 
     cv2.bilateralFilter(dst,average_square_size,sigma_color,sigma_metric)
            hsv_img = cv2.cvtColor(img_bilateral, cv2.COLOR_BGR2HSV)                                 #HSVモデルに変更
            img_gly = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)#グレースケール
    
# Remove background - Set pixels further than clipping_distance to grey
            white_color = 255 #RGBでの白色
            depth_image_2d = np.dstack((dst_depth,dst_depth,dst_depth)) # 配列同士を奥行きで重ねる。RGBに対してdepth情報追加
            bg_removed = np.where((depth_image_2d > clipping_distance) | 
(depth_image_2d < 0), depth_gry, hsv_img)#引数1 条件式  #引数2 条件合致時の置き換え #引数3 条件不一致時の置き換え 
    
#2値処理
            lower = (0, 7, 140)                        #引数1色相  引数２彩度   引数3明度   色相は赤→黄→緑→水色→青→紫でループする  
            upper = (359, 255, 255)
            bin_img = cv2.inRange(bg_removed, lower, upper) #2値化   引数1 画像指定  引数2 下限の色  引数3 上限の色
            contours, hierarchy = cv2.findContours(bin_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE ) #輪郭検出
            contours = list(filter(lambda x: cv2.contourArea(x) > 200000, contours))                     # 小さい輪郭は誤検出として削除する
    
 #輪郭処理
            xl=20  #左判定ポイント座標設定パラメータ
            xr=w-xl   #右判定ポイント座標設定パラメータ
            yr1=yl1=5 #判定ポイント座標設定パラメータ
            yr2=yl2=7  #判定ポイント座標設定パラメータ
     
            if 255==bin_img[yr1,xl] and 255==bin_img[yl1,xr]: #前判定ゾーンで監視+距離データ(depth_image_3d)で判断する?
                 print('止まってください')
                 data = "止まってください" # 送信データの作成
                    
             elif 255==bin_img[yr1,xr] and 255==bin_img[yr2,xr]: #引数1 y座標  引数2 x座標
                 print('左に行ってください')  
                 data = "左に行ってください" # 送信データの作成
         
            elif 255==bin_img[yl1,xl] and 255==bin_img[yl2,xl] : #左判定ゾーンで監視
                print('右に行ってください')
                data = "右に行ってください" # 送信データの作成
            else:
                print('正常')
                data = "正常" # 送信データの作成 

#処理画像表示部分
            cv2.namedWindow('RGB', cv2.WINDOW_NORMAL)
            cv2.imshow('RGB',dst)#2値化(bin_img) RGB(dst)
            cv2.namedWindow('RGB-D', cv2.WINDOW_NORMAL)
            cv2.imshow('RGB-D',bin_img)#2値化(bin_img) RGB(dst)

 #処理画像保存部分
            c+=1
            write_file_name = f'RGB{c:05d}.jpg'
            write_file_name1 = f'RGB-D{c:05d}.jpg'
            cv2.imwrite('./AIdata002/'+write_file_name, dst) 
            cv2.imwrite('./AIdata002/'+write_file_name1, img_gly) 
            key = cv2.waitKey(5)
    # Press esc or 'q' to close the image window
            if key & 0xFF == ord('q') or key == 27:
                 cv2.destroyAllWindows()
                 break
        finally:
            pipeline.stop()

#ざっくり解説
　主な流れ：　1.カメラの初期設定
　　　　　　　2.画像データ（RGB-D）を取得
　　　　　　　3.データのリサイズ+平滑処理
　　　　　　　4.色相に応じた2値化処理
　　　　　　　5.4箇所のピクセルで白or黒の検出して方向修正処理
　　　　　　　6.カメラでセンシングした画像の表示&ファイル保存