search

ShunjiHashimoto / tang

収穫サポートロボットTANG
2 stars 1 forks source link

pyrealsenseの処理が重たい #206

Closed ShunjiHashimoto closed 2 years ago

ShunjiHashimoto commented 2 years ago

https://github.com/ShunjiHashimoto/tang/issues/31 https://github.com/ShunjiHashimoto/tang/issues/197

ShunjiHashimoto commented 2 years ago

https://dev.classmethod.jp/articles/distance-detection-realsence/

ShunjiHashimoto commented 2 years ago

まずは最もシンプルなpyrealsenseの使い方で処理どれだけかかるか試してみよう

ShunjiHashimoto commented 2 years ago 
$ python3 pyrealsense_test.py 
2.5143721103668213
0.32514071464538574

ただpyrealsenseを動かしただけでもこれくらい これプラス人物検出で0.2秒くらいか

ShunjiHashimoto commented 2 years ago

背景処理自体にはそんな時間は取られていない

ShunjiHashimoto commented 2 years ago

FPSの組み合わせ https://github.com/IntelRealSense/librealsense/issues/1957#issuecomment-774302396

ShunjiHashimoto commented 2 years ago

元ネタ：fpsがすくにあ https://github.com/IntelRealSense/librealsense/issues/6313 pyrealsenseがcpuで処理行っているからGPUも使ってほしい https://github.com/IntelRealSense/librealsense/issues/4905#issuecomment-533854888 これをビルドしたらいけるかも？ https://github.com/IntelRealSense/librealsense/issues/6964#issuecomment-665650908 ビルドするためには下記のようにすればよいらしい https://github.com/jetsonhacks/buildLibrealsense2TX/issues/13#issuecomment-573976359 エラーが発生

ubuntu@xavier:~/librealsense/build$ cmake ../ -DFORCE_RSUSB_BACKEND=ON -DBUILD_PYTHON_BINDINGS:bool=true -DPYTHON_EXECUTABLE=...
-- Checking internet connection...
-- Internet connection identified
-- Info: REALSENSE_VERSION_STRING=2.51.1
-- Setting Unix configurations
-- Info: Building with CUDA requires CMake v3.8+
-- The CUDA compiler identification is NVIDIA 10.2.300
-- Check for working CUDA compiler: /usr/local/cuda/bin/nvcc
-- Check for working CUDA compiler: /usr/local/cuda/bin/nvcc -- works
-- Detecting CUDA compiler ABI info
-- Detecting CUDA compiler ABI info - done
-- Found CUDA: /usr/local/cuda (found version "10.2") 
-- CUDA_LIBRARIES: /usr/local/cuda/include /usr/local/cuda/lib64/libcudart_static.a;dl;/usr/lib/aarch64-linux-gnu/librt.so;/usr/local/cuda/lib64/libcusparse.so;/usr/local/cuda/lib64/libcublas.so
-- Building libcurl enabled
-- using RS2_USE_LIBUVC_BACKEND
-- Configuring done
-- Generating done
-- Build files have been written to: /home/ubuntu/librealsense/build/external-projects/pybind11
[ 12%] Performing update step for 'pybind11'
[ 25%] No configure step for 'pybind11'
[ 37%] No build step for 'pybind11'
[ 50%] No install step for 'pybind11'
[ 62%] Completed 'pybind11'
[100%] Built target pybind11
-- pybind11 v2.6.2 
-- Found PythonInterp: /home/ubuntu/librealsense/build/... (found version "1.4") 
CMake Error at build/third-party/pybind11/tools/FindPythonLibsNew.cmake:133 (message):
  Python config failure:

解決策、これでpythonのパスを通す https://support.intelrealsense.com/hc/en-us/community/posts/4404187644819-Error-while-installing-Real-Sense-software-to-tthe-Mac-OS

ShunjiHashimoto commented 2 years ago 
# ストリーミング開始
pipeline = rs.pipeline()
profile = pipeline.start(config)
color_sensor = profile.get_device().query_sensors()[1]
color_sensor.set_option(rs.option.enable_auto_exposure, 0)
color_sensor.set_option(rs.option.exposure, 1000) # microseconds
color_sensor.set_option(rs.option.gain, 64)
color_sensor.set_option(rs.option.frames_queue_size, 1)

# 距離[m] = depth * depth_scale 
depth_sensor = profile.get_device().first_depth_sensor()
depth_sensor.set_option(rs.option.exposure, 400) # microseconds
depth_sensor.set_option(rs.option.gain, 16)
depth_sensor.set_option(rs.option.frames_queue_size, 1)
depth_scale = depth_sensor.get_depth_scale()
clipping_distance_in_meters = 0.4 # 40cm以内を検出
clipping_distance = clipping_distance_in_meters / depth_scale

exposureをオフにして各自設定したら0.2秒くらいになったけど、まだおそい、、30FPSほしい（0.0333）

ShunjiHashimoto commented 2 years ago

コードは多分問題ない気がする https://qiita.com/SatoshiGachiFujimoto/items/3c335d9dcc7e8cc211ff 変なことは自分は書いてないので、librealsense周りのビルドとか、GPUとかかな

ShunjiHashimoto commented 2 years ago

結論 推論速度が10倍になった

Jetson Nano 1loop 0.3 ~ 0.5 推論速度 20FPS
Jetson Xavier
1loop 0.09 ~ 0.1 推論速度 200FPS

やったこと

pyrealsenseの中のpython, exampleのなかのopencv-exampleを参考にコードを書き換えた。 下記の

WIDTH = 640 HEIGHT = 480

Configure depth and color streams

pipeline = rs.pipeline() config = rs.config()

Get device product line for setting a supporting resolution

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, 15)

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

Start streaming

pipeline.start(config)

profile = pipeline.start(config)

距離[m] = depth * depth_scale

depth_sensor = profile.get_device().first_depth_sensor()

depth_sensor.set_option(rs.option.exposure, 400) # microseconds

depth_scale = depth_sensor.get_depth_scale() clipping_distance_in_meters = 0.8 # 40cm以内を検出 clipping_distance = clipping_distance_in_meters / depth_scale

Alignオブジェクト生成

align_to = rs.stream.color align = rs.align(align_to)

threshold = (WIDTH HEIGHT 3) * 0.95

try: while True:

時間計測開始

    time_sta = time.time()
    frames = pipeline.wait_for_frames()
    depth_frame = frames.get_depth_frame()
    color_frame = frames.get_color_frame()
    if not depth_frame or not color_frame:
        continue
    # aligned_frames = align.process(frames)
    # color_frame = aligned_frames.get_color_frame()
    # depth_frame = aligned_frames.get_depth_frame()
    # if not depth_frame or not color_frame:
    #     continue

    color_image = np.asanyarray(color_frame.get_data())
    depth_image = np.asanyarray(depth_frame.get_data())

    # clipping_distance_in_metersm以内を画像化
    white_color = 255 # 背景色
    depth_image_3d = np.dstack((depth_image, depth_image, depth_image))
    bg_removed = np.where((depth_image_3d > clipping_distance) | (depth_image_3d <= 0), white_color, color_image)
    # 背景色となっているピクセル数をカウント
    white_pic = np.sum(bg_removed == 255)
    # 背景色が一定値以下になった時に、「検出」を表示する
    if(threshold > white_pic):
        print("検出 {}".format(white_pic))
    else:
        print("{}".format(white_pic))
    depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
    depth_colormap_dim = depth_colormap.shape
    color_colormap_dim = color_image.shape

    # If depth and color resolutions are different, resize color image to match depth image for display
    if depth_colormap_dim != color_colormap_dim:
        resized_color_image = cv2.resize(color_image, dsize=(depth_colormap_dim[1], depth_colormap_dim[0]), interpolation=cv2.INTER_AREA)
        images = np.hstack((resized_color_image, depth_colormap))
    else:
        images = np.hstack((color_image, depth_colormap))

    # images = np.hstack((bg_removed, color_image))
    # cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
    images = np.hstack((bg_removed, color_image))
    cv2.imshow('Frames', images)
    # 時間計測終了
    time_end = time.time()
    # 経過時間（秒）
    tim = time_end- time_sta
    print(tim)

    if cv2.waitKey(1) & 0xff == 27:
        break

finally:

ストリーミング停止

pipeline.stop()
cv2.destroyAllWindows()
とくに
```python
# aligned_frames = align.process(frames)
        # color_frame = aligned_frames.get_color_frame()
        # depth_frame = aligned_frames.get_depth_frame()
        # frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()

align~をすると処理が滅茶重くなっていたので主な原因はコレの模様です。 depthとcolorの画角を合わせる処理らしい これが人物追従にどう影響するかはちゃんとみないとね、 これがあると1ループ0.5sくらいかかってしまう。

ShunjiHashimoto commented 2 years ago

https://twitter.com/devemin/status/1468787581752332291?lang=ar-x-fm devさんも同じ現象

ShunjiHashimoto commented 2 years ago

https://qiita.com/idev_jp/items/3eba792279d836646664 これも参考にならなくもない

ShunjiHashimoto commented 2 years ago

https://github.com/IntelRealSense/librealsense/issues/8362 こっちはauto exposruを斬れと書いている

ShunjiHashimoto commented 2 years ago

現状報告、まだ戦いは終わっていなかった。 まず、alingnをして画角を合わせるせいで処理が重たい。 なのでそれがなければよいのだが、やっぱり必要そう。人検出に影響が出る alignありにすると、なんと現状 Jetson 1 loop 0.2s FPS 100くらい なぜか200だったのが半分になってしまった。なぜ。

あとJetsonのモードも今まで30W ALLでやっていたが 30W 2core 4core 6core ALLの順番で処理が早くなることが判明、ちゃんと調べるべき 今日はこのへんで、来週はデバッグ頑張るべ

ShunjiHashimoto commented 2 years ago

https://medium.com/@smart-design-techology/hand-detection-in-3d-space-888433a1c1f3 同じことやってる人いた！この人のコードを見たらなにかわかるかもねんん

ShunjiHashimoto commented 2 years ago

上記のコメントのサイトのとおりにやればうまくできた。 https://medium.com/@smart-design-techology/hand-detection-in-3d-space-888433a1c1f3 注意点としては、

$ cmake ../ -DFORCE_RSUSB_BACKEND=ON -DBUILD_PYTHON_BINDINGS:bool=true -DPYTHON_EXECUTABLE=/home/[username]/archiconda3/envs/[conda env name]/bin/python -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=true -DBUILD_GRAPHICAL_EXAMPLES=true -DBUILD_WITH_CUDA:bool=true

ここは、DPYTHOのところは、 https://support.intelrealsense.com/hc/en-us/community/posts/4404187644819-Error-while-installing-Real-Sense-software-to-tthe-Mac-OS このissueのところで、 which pythonと打ってでてきたpythonのパスをコマンドのusername~のところに書けば動く

ShunjiHashimoto commented 2 years ago

学習モデルが見つかりませんというエラーが出たが、下記のReadMeのとおりに再度ビルドし直す。 https://github.com/dusty-nv/jetson-inference/blob/master/docs/building-repo-2.md

ShunjiHashimoto commented 2 years ago 
$ python3 detectnet.py /dev/video2
jetson.utils -- compiled without NumPy array conversion support (warning)
jetson.utils -- if you wish to have support for converting NumPy arrays,
jetson.utils -- first run 'sudo apt-get install python-numpy python3-numpy'

https://github.com/dusty-nv/jetson-inference/issues/1017 このエラーは上記の通りにやる

ShunjiHashimoto commented 2 years ago 
時間 0.0970759391784668
[INFO] [1664471060.195763]: human_input: x:0.476000, y:-0.012837, z:-0.086232
[WARN] [1664471060.201020]: estimated: x:0.481252, y:-0.017002, z:-0.086232
時間 0.07934331893920898
[INFO] [1664471060.274422]: human_input: x:0.474000, y:-0.030011, z:-0.085086
[WARN] [1664471060.282784]: estimated: x:0.477703, y:-0.017002, z:-0.085086
時間 0.08331108093261719
[INFO] [1664471060.358757]: human_input: x:0.459000, y:-0.060912, z:-0.087706
[WARN] [1664471060.365449]: estimated: x:0.472668, y:-0.017003, z:-0.087706
時間 0.08374905586242676
[INFO] [1664471060.441540]: human_input: x:0.460000, y:-0.046605, z:-0.087137
[WARN] [1664471060.447137]: estimated: x:0.470388, y:-0.017003, z:-0.087137
時間 0.08056163787841797

無事動いた！ 制御周期はだいたい0.08~0.1くらいになった！

ShunjiHashimoto commented 2 years ago

jetson

[INFO] [1664472768.336126]: human_input: x:0.653200, y:-0.004665, z:0.004794
0.15120720863342285
out layer [5923.23210467] 観測値z [ 2.46437286e+00 -1.26052684e-02 -1.35763507e-03]
[INFO] [1664472768.488681]: human_input: x:2.463000, y:-0.001312, z:0.005859
0.1525437831878662
out layer [8116.53428319] 観測値z [ 3.46986311  0.06724026 -0.50750902]
[INFO] [1664472768.640889]: human_input: x:3.477400, y:0.072835, z:-0.520717
0.14789295196533203
out layer [2913.87736681] 観測値z [ 1.23486727 -0.00791549  0.01396948]
[INFO] [1664472768.791249]: human_input: x:1.240600, y:-0.006810, z:0.005003
0.15008974075317383
out layer [1345.16240378] 観測値z [3.71640192 0.0195308  0.0837381 ]
[INFO] [1664472768.941529]: human_input: x:3.720800, y:0.034902, z:0.076528
0.14658355712890625
out layer [59.33955571] 観測値z [ 0.68822866 -0.02287754  0.00620543]
[INFO] [1664472769.096860]: human_input: x:0.692400, y:-0.007233, z:0.002792
0.14960217475891113
ShunjiHashimoto commented 2 years ago

改善後

[INFO] [1664497475.505705]: human_input: x:0.603000, y:0.004660, z:-0.002554
0.14654946327209473
[INFO] [1664497475.642521]: human_input: x:0.603200, y:0.003665, z:-0.003552
0.13480901718139648
[INFO] [1664497475.778922]: human_input: x:0.602600, y:0.003661, z:-0.003548
0.14845609664916992
[INFO] [1664497475.925051]: human_input: x:0.599800, y:0.002653, z:0.000435
0.13725519180297852
[INFO] [1664497476.066351]: human_input: x:0.590800, y:0.003590, z:-0.007387
0.14749956130981445

あまり変わらず

ShunjiHashimoto commented 2 years ago

realsenseのテストscritp

# -*- coding: utf-8 -*-
import pyrealsense2 as rs
import numpy as np
import cv2
import time

WIDTH = 640
HEIGHT = 480

# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()

# Get device product line for setting a supporting resolution
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, 15)

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

# Start streaming
# pipeline.start(config)
profile = pipeline.start(config)

# 距離[m] = depth * depth_scale 
depth_sensor = profile.get_device().first_depth_sensor()
# depth_sensor.set_option(rs.option.exposure, 400) # microseconds
depth_scale = depth_sensor.get_depth_scale()
clipping_distance_in_meters = 0.8 # 40cm以内を検出
clipping_distance = clipping_distance_in_meters / depth_scale

# Alignオブジェクト生成
align_to = rs.stream.color
align = rs.align(align_to)

threshold = (WIDTH * HEIGHT * 3) * 0.95

try:
    while True:
        # 時間計測開始
        time_sta = time.time()
        frames = pipeline.wait_for_frames()
        # depth_frame = frames.get_depth_frame()
        # color_frame = frames.get_color_frame()
        # if not depth_frame or not color_frame:
        #     continue
        aligned_frames = align.process(frames)
        color_frame = aligned_frames.get_color_frame()
        depth_frame = aligned_frames.get_depth_frame()
        if not depth_frame or not color_frame:
            continue

        color_image = np.asanyarray(color_frame.get_data())
        depth_image = np.asanyarray(depth_frame.get_data())

        # clipping_distance_in_metersm以内を画像化
        white_color = 255 # 背景色
        depth_image_3d = np.dstack((depth_image, depth_image, depth_image))
        bg_removed = np.where((depth_image_3d > clipping_distance) | (depth_image_3d <= 0), white_color, color_image)
        # 背景色となっているピクセル数をカウント
        white_pic = np.sum(bg_removed == 255)
        # 背景色が一定値以下になった時に、「検出」を表示する
        if(threshold > white_pic):
            print("検出 {}".format(white_pic))
        else:
            print("{}".format(white_pic))
        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
        depth_colormap_dim = depth_colormap.shape
        color_colormap_dim = color_image.shape

        # If depth and color resolutions are different, resize color image to match depth image for display
        if depth_colormap_dim != color_colormap_dim:
            resized_color_image = cv2.resize(color_image, dsize=(depth_colormap_dim[1], depth_colormap_dim[0]), interpolation=cv2.INTER_AREA)
            images = np.hstack((resized_color_image, depth_colormap))
        else:
            images = np.hstack((color_image, depth_colormap))

        # images = np.hstack((bg_removed, color_image))
        # cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
        images = np.hstack((bg_removed, color_image))
        cv2.imshow('Frames', images)
        # 時間計測終了
        time_end = time.time()
        # 経過時間（秒）
        tim = time_end- time_sta
        print(tim)

        if cv2.waitKey(1) & 0xff == 27:
            break

finally:
    # ストリーミング停止
    pipeline.stop()
    cv2.destroyAllWindows()