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.カメラでセンシングした画像の表示&ファイル保存