2021年1月12日火曜日

ubuntu 20.04+ros2 foxyでorab_slam2_rosを動作させてみた

ubuntu 20.04とros2 foxyでカメラを動作させてorb_slam2_rosを試してみた際のメモ。 カメラで試したのはros2_v4l2_cameraとopencv_cam。 ほぼリンク先の通り実行しただけ。

opencvの準備

ubuntu 20.04標準のopencvではバージョン起因と思われるエラーが出たので、 v4.5.0をソースからビルドした。 特別問題なくビルドできた。

cd <path-to-opencv-download-dir>
unzip opencv-4.5.0.zip
cd opencv-4.5.0
mkdir build
cd build
cmake .. -GNinja -DCMAKE_INSTALL_PREFIX=<opencv-installation-path>/local
ninja -j4
ninja install

私の環境では既にopencv3.4.12が入っているので、v4.5.0が先に見つかるように環境変数を設定する。 colcon実行前に以下のコマンドが必要。

export PATH=<opencv-installation-path>/local/bin:$PATH
export LD_LIBRARY_PATH=<opencv-installation-path>/local/lib:$LD_LIBRARY_PATH
export PYTHONPATH=<opencv-installation-path>/local/lib/python3.8/dist-packages:$PYTHONPATH

ros2_v4l2_cameraの準備

こちらは記載の通りに実行して特に問題なかった。 ~/ros_work/srcにリポジトリをクローンする。

使用したhashは以下の通り

ros2_v4l2_camera:65b072565f8e4520d166b725d91d5f31acd97610
image_common:4c5053ecf703a58c25a712d1d4a80012f5076a1f
image_transport_plugins:0565f23dc955fa2d56cffcfebc63de5a97cd005f
vision_opencv:2026f6d66833e0830f9ce69004d43c9abd1e5fa4

opencv_camの準備

ここを参考にしてビルドした。 image_pipelineのビルドはros2ブランチのhash:b4fbbae6463fb3baaa4b36a0a7517d0b0395e837を使用する。 foxyっぽいブランチを使用すると2021/1/12時点ではcalibrationを実行する際にros-argsを指定すると実行できない問題がある模様。 (https://github.com/ros-perception/image_pipeline/pull/597) image_commonはros2_v4ls_cameraと同じ。 ~/ros_work/srcにリポジトリをクローンする。

使用したhashは以下の通り。

opencv_cam:f4487b0de651a4323a535f8e8d0e86b147cc81c2
ros2_shared:02433ef4f873876c3dd3ab2925987cf04d224660
image_common:4c5053ecf703a58c25a712d1d4a80012f5076a1f
image_pipeline:b4fbbae6463fb3baaa4b36a0a7517d0b0395e837

ビルド

パスの設定をしてビルドする。

cd ~/ros_work
export PATH=<opencv-installation-path>/local/bin:$PATH
export LD_LIBRARY_PATH=<opencv-installation-path>/local/lib:$LD_LIBRARY_PATH
export PYTHONPATH=<opencv-installation-path>/local/lib/python3.8/dist-packages:$PYTHONPATH
colcon build --symlink-install

videoの出力

cam2imageを試す場合は以下。 /dev/video0のみが出力できる。

source ~/ros_work/install/setup.sh
ros2 run image_tools cam2image

v4l2_cameraを試す場合は以下。 video_deviceのパラメータで指定可能。 videoデバイスが複数合ってどれを使えばいいかわからない場合は、 videoデバイスの情報の調べ方を使って調べられる。 USBなら挿して増えたものでいいとは思う。 eloquent以前のビルドはできそうになかったので、 ultra96v2への採用は見送った。

source ~/ros_work/install/setup.sh
ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:=/dev/video14

opencv_camを試す場合は以下。 indexでvideoデバイスファイルの番号を指定できる。

source ~/ros_work/install/setup.sh
ros2 run opencv_cam opencv_cam_main --ros-args --param index:=14

videoデバイスの情報の調べ方

v4l2-ctlを使用する。 ない場合にはv4l-utilsをインストールすると使用できる。

sudo apt install v4l-utils

logicoolのc270nに試してみた結果は以下の通り。

# v4l2-ctl -d /dev/video14 --all
driver Info:
    Driver name      : uvcvideo
    Card type        : UVC Camera (046d:0825)
    Bus info         : usb-0000:00:14.0-1.1
    Driver version   : 5.8.18
    Capabilities     : 0x84a00001
        Video Capture
        Metadata Capture
        Streaming
        Extended Pix Format
        Device Capabilities
    Device Caps      : 0x04200001
        Video Capture
        Streaming
        Extended Pix Format
Media Driver Info:
    Driver name      : uvcvideo
    Model            : UVC Camera (046d:0825)
    Serial           : 98251310
    Bus info         : usb-0000:00:14.0-1.1
    Media version    : 5.8.18
    Hardware revision: 0x00000012 (18)
    Driver version   : 5.8.18
Interface Info:
    ID               : 0x03000002
    Type             : V4L Video
Entity Info:
    ID               : 0x00000001 (1)
    Name             : UVC Camera (046d:0825)
    Function         : V4L2 I/O
    Flags         : default
    Pad 0x01000007   : 0: Sink
      Link 0x02000019: from remote pad 0x100000a of entity 'Extension 4': Data, Enabled, Immutable
Priority: 2
Video input : 0 (Camera 1: ok)
Format Video Capture:
    Width/Height      : 640/480
    Pixel Format      : 'YUYV' (YUYV 4:2:2)
    Field             : None
    Bytes per Line    : 1280
    Size Image        : 614400
    Colorspace        : sRGB
    Transfer Function : Default (maps to sRGB)
    YCbCr/HSV Encoding: Default (maps to ITU-R 601)
    Quantization      : Default (maps to Limited Range)
    Flags             : 
Crop Capability Video Capture:
    Bounds      : Left 0, Top 0, Width 640, Height 480
    Default     : Left 0, Top 0, Width 640, Height 480
    Pixel Aspect: 1/1
Selection Video Capture: crop_default, Left 0, Top 0, Width 640, Height 480, Flags: 
Selection Video Capture: crop_bounds, Left 0, Top 0, Width 640, Height 480, Flags: 
Streaming Parameters Video Capture:
    Capabilities     : timeperframe
    Frames per second: 30.000 (30/1)
    Read buffers     : 0
                     brightness 0x00980900 (int)    : min=0 max=255 step=1 default=128 value=128
                       contrast 0x00980901 (int)    : min=0 max=255 step=1 default=32 value=32
                     saturation 0x00980902 (int)    : min=0 max=255 step=1 default=32 value=32
 white_balance_temperature_auto 0x0098090c (bool)   : default=1 value=1
                           gain 0x00980913 (int)    : min=0 max=255 step=1 default=64 value=64
           power_line_frequency 0x00980918 (menu)   : min=0 max=2 default=2 value=2
                0: Disabled
                1: 50 Hz
                2: 60 Hz
      white_balance_temperature 0x0098091a (int)    : min=0 max=10000 step=10 default=4000 value=4000 flags=inactive
                      sharpness 0x0098091b (int)    : min=0 max=255 step=1 default=24 value=24
         backlight_compensation 0x0098091c (int)    : min=0 max=1 step=1 default=0 value=0
                  exposure_auto 0x009a0901 (menu)   : min=0 max=3 default=3 value=3
                1: Manual Mode
                3: Aperture Priority Mode
              exposure_absolute 0x009a0902 (int)    : min=1 max=10000 step=1 default=166 value=166 flags=inactive
         exposure_auto_priority 0x009a0903 (bool)   : default=0 value=1

videoの確認

上記でvideoの出力したものはrqt_image_viewで確認できる。 rviz2だとカメラ画像はうまく表示されなかった。(camera_infoの出力が必要とのこと)

ros2 run rqt_image_view rqt_image_view

 

calibration

カメラのキャリブレーションしないとopencv_camを実行した際にcamera_infoのtopicを出力できない旨のエラーメッセージが出力される。

[ERROR] [1610449586.191007718] [camera_calibration_parsers]: Failed to detect content in .ini file
[ERROR] [1610449586.191095947] [opencv_cam]: cannot get camera info, will not publish

キャリブレーション方法はこちらに記載の内容に従って行った。 途中でフリーズしてCalibrateボタンが押せないことが何度かあったが、頑張ってプログラムを完了させた。

#ros2 run camera_calibration cameracalibrator \
  --size=8x6 \
  --square=0.063 \
  --approximate=0.3 \
  --no-service-check \
  --ros-args --remap /image:=/image_raw
*** Added sample 1, p_x = 0.479, p_y = 0.527, p_size = 0.602, skew = 0.051
*** Added sample 2, p_x = 0.790, p_y = 0.645, p_size = 0.634, skew = 0.083
*** Added sample 3, p_x = 0.758, p_y = 0.556, p_size = 0.634, skew = 0.164
*** Added sample 4, p_x = 0.599, p_y = 0.417, p_size = 0.627, skew = 0.155
*** Added sample 5, p_x = 0.529, p_y = 0.281, p_size = 0.622, skew = 0.103
*** Added sample 6, p_x = 0.435, p_y = 0.000, p_size = 0.627, skew = 0.128
*** Added sample 7, p_x = 0.463, p_y = 0.776, p_size = 0.621, skew = 0.057
*** Added sample 8, p_x = 0.630, p_y = 0.905, p_size = 0.627, skew = 0.056
*** Added sample 9, p_x = 0.465, p_y = 0.997, p_size = 0.629, skew = 0.101
*** Added sample 10, p_x = 0.787, p_y = 0.888, p_size = 0.667, skew = 0.104
*** Added sample 11, p_x = 0.336, p_y = 0.914, p_size = 0.647, skew = 0.025
*** Added sample 12, p_x = 0.355, p_y = 0.735, p_size = 0.666, skew = 0.005
*** Added sample 13, p_x = 0.460, p_y = 0.585, p_size = 0.688, skew = 0.012
*** Added sample 14, p_x = 0.255, p_y = 0.577, p_size = 0.683, skew = 0.032
*** Added sample 15, p_x = 0.371, p_y = 0.504, p_size = 0.706, skew = 0.054
*** Added sample 16, p_x = 0.640, p_y = 0.781, p_size = 0.737, skew = 0.073
*** Added sample 17, p_x = 0.402, p_y = 0.322, p_size = 0.719, skew = 0.036
*** Added sample 18, p_x = 0.534, p_y = 0.101, p_size = 0.747, skew = 0.069
*** Added sample 19, p_x = 0.689, p_y = 0.225, p_size = 0.764, skew = 0.129
*** Added sample 20, p_x = 0.632, p_y = 0.539, p_size = 0.733, skew = 0.140
*** Added sample 21, p_x = 0.527, p_y = 0.628, p_size = 0.711, skew = 0.133
*** Added sample 22, p_x = 0.241, p_y = 0.822, p_size = 0.620, skew = 0.040
*** Added sample 23, p_x = 0.524, p_y = 0.801, p_size = 0.745, skew = 0.015
*** Added sample 24, p_x = 0.603, p_y = 0.734, p_size = 0.669, skew = 0.157
*** Added sample 25, p_x = 0.404, p_y = 0.131, p_size = 0.690, skew = 0.103
*** Added sample 26, p_x = 0.301, p_y = 0.048, p_size = 0.690, skew = 0.082
*** Added sample 27, p_x = 0.743, p_y = 0.106, p_size = 0.692, skew = 0.131
*** Added sample 28, p_x = 0.788, p_y = 0.305, p_size = 0.689, skew = 0.128
*** Added sample 29, p_x = 0.423, p_y = 0.621, p_size = 0.631, skew = 0.084
*** Added sample 30, p_x = 0.638, p_y = 0.191, p_size = 0.651, skew = 0.117
*** Added sample 31, p_x = 0.410, p_y = 0.404, p_size = 0.629, skew = 0.059
*** Added sample 32, p_x = 0.700, p_y = 0.411, p_size = 0.678, skew = 0.244
*** Added sample 33, p_x = 0.577, p_y = 0.321, p_size = 0.674, skew = 0.193
*** Added sample 34, p_x = 0.416, p_y = 0.468, p_size = 0.664, skew = 0.171
*** Added sample 35, p_x = 0.348, p_y = 0.709, p_size = 0.778, skew = 0.065
*** Added sample 36, p_x = 0.546, p_y = 0.666, p_size = 0.784, skew = 0.035
*** Added sample 37, p_x = 0.545, p_y = 0.369, p_size = 0.769, skew = 0.006
*** Added sample 38, p_x = 0.640, p_y = 0.493, p_size = 0.598, skew = 0.063
*** Added sample 39, p_x = 0.814, p_y = 0.795, p_size = 0.610, skew = 0.071
*** Added sample 40, p_x = 0.730, p_y = 0.839, p_size = 0.600, skew = 0.166
*** Added sample 41, p_x = 0.587, p_y = 0.679, p_size = 0.570, skew = 0.052
D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 0.0, 0.0, 1.0]
R =  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000


D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 0.0, 0.0, 1.0]
R =  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000


('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')
D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 0.0, 0.0, 1.0]
R =  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000

/tmp/calibrationdata.tar.gzに保存されたものを使用する。 解凍すると、ost.txtがあり、これがiniファイルとして使用できる。(コンソールの結果も同じ) なので、camera-info-logicool-c270n.iniとして保存した。 使うときには以下の通り。 ただし、rviz2では表示されなかった。あと何がいるんだ？

ros2 run opencv_cam opencv_cam_main --ros-args --param index:=14 --param camera_info_path:=camera-info-logicool-c270n.ini

orb_slam2実行

以下のコマンドでorb_slam2_rosを実行する。

ros2 run orb_slam2_ros orb_slam2_ros_mono --ros-args --remap /camera/image_raw:=/image_raw --remap /camera/camera_info:=/camera_info --param params_file:=install/orb_slam2_ros/share/orb_slam2_ros/ros/config/params_d435_mono.yaml --param voc_file:=install/orb_slam2_ros/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt

ultra96v2で実行する場合は以下。機能の実装方法は次回。

ros2 run orb_slam2_ros orb_slam2_ros_mono --ros-args --remap /camera/image_raw:=/image_raw --remap /camera/camera_info:=/camera_info --param params_file:=/usr/share/orb_slam2_ros/ros/config/params_d435_mono.yaml --param voc_file:=/usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt

以下のような結果が得られた。 なんだか微妙な感じだけどキャリブレーションうまくいってないのか？


 

参考

http://joe.ash.jp/program/ros/tutorial/tutorial_camera.htm
https://medium.com/swlh/raspberry-pi-ros-2-camera-eef8f8b94304
https://jeffzzq.medium.com/ros2-image-pipeline-tutorial-3b18903e7329

投稿者 時刻:
ラベル: , , ,

0 件のコメント:

コメントを投稿

登録: コメントの投稿 (Atom)