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
