スマホ二台を使ったステレオカメラでRTAB-MAPしようとして失敗
androidスマホ(BV5500)を二台横に並べてステレオカメラをつくりRTAB-MAPを行おうとしました。
3Dプリンターを使ってスマホを横に２つ並べる枠を作りました。
androidからストリーム配信した動画をPCのROS上で受信していきます。画像の受信は「video_stream_opencv」というパッケージを使って行います。二台同時に受信するため、launchファイルを作成します。
$ cd /opt/ros/kinetic/share/video_stream_opencv/launch/ $ sudo cp camera.launch stereo_camera.launch $ sudo gedit stereo_camera.launch
テキストエディタで32行目を以下に書き換えます。
<group ns="/stereo/$(arg camera_name)">
$ mkdir ~/ros_launch_files && cd ~/ros_launch_files $ touch stereo_rtsp_VB5500.launch $ gedit stereo_rtsp_VB5500.launch
テキストエディタで以下の内容をコピペして保存します。
<?xml version="1.0"?>
<launch>
<include file="$(find video_stream_opencv)/launch/stereo_camera.launch" >
<!-- node name and ros graph name -->
<arg name="camera_name" value="right" />
<arg name="video_stream_provider" value="rtsp://192.168.0.22:5540/ch0" />
<arg name="buffer_queue_size" value="1000" />
<arg name="fps" value="30" />
<arg name="frame_id" value="rtsp_frame_right" />
<arg name="camera_info_url" value="file:///home/user_name/calibrate_file/VB5500_stereo_streamVideo_right.yaml" />
<arg name="flip_horizontal" value="false" />
<arg name="flip_vertical" value="false" />
<arg name="visualize" value="true" />
</include>
<include file="$(find video_stream_opencv)/launch/stereo_camera.launch" >
<!-- node name and ros graph name -->
<arg name="camera_name" value="left" />
<arg name="video_stream_provider" value="rtsp://192.168.0.28:5540/ch0" />
<arg name="buffer_queue_size" value="1000" />
<arg name="fps" value="30" />
<arg name="frame_id" value="rtsp_frame_left" />
<arg name="camera_info_url" value="file:///home/user_name/calibrate_file/VB5500_stereo_streamVideo_left.yaml" />
<arg name="flip_horizontal" value="false" />
<arg name="flip_vertical" value="false" />
<arg name="visualize" value="true" />
</include>
</launch>受信に成功したらキャリブレーションファイルを作成します。
sh090.hatenablog.com
以前書いたページを参考に、以下のコマンドを実行してキャリブレーションを行います。yamlファイルが２つ作成されます。さっき作ったlaunchファイルのcamera_info_urlにファイルの保存先を入力します。
$ rosrun camera_calibration cameracalibrator.py --approximate 0.1 --size 8x6 --square 0.025 right:=/right/image_raw left:=/left/image_raw right_camera:=/right left_camera:=/left
<arg name="camera_info_url" value="file:///home/user_name/calibrate_file/VB5500_stereo_streamVideo_left.yaml" />
ビジュアルオドメトリのライブラリであるviso2(https://github.com/srv/viso2)を導入します。
$ cd ~/ros_ws/src $ git clone https://github.com/srv/viso2.git $ cd .. $ catkin_make
RTAB-MAPを実行するlaunchファイルを作成します。
cd ~/ros_launch_files touch stereo_RTAB.launch gedit stereo_RTAB.launch
テキストエディタで以下の内容をコピペして保存します。
<launch>
<!-- Run the ROS package stereo_image_proc for image rectification and disparity computation -->
<group ns="stereo">
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<param name="approximate_sync" value="true"/>
</node>
<!-- Disparity to depth -->
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
</group>
<!-- Odometry: Run the viso2_ros package -->
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
<remap from="image" to="image_rect"/>
<param name="base_link_frame_id" value="/base_link"/>
<param name="odom_frame_id" value="/odom"/>
<param name="ref_frame_change_method" value="1"/>
<param name="queue_size" type="int" value="20"/>
<param name="approximate_sync" type="bool" value="true"/>
</node>
<group ns="rtabmap">
<!-- Visual SLAM -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<remap from="rgb/image" to="/stereo/left/image_rect"/>
<remap from="rgb/camera_info" to="/stereo/left/camera_info"/>
<remap from="depth/image" to="/stereo/depth"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="frame_id" type="string" value="/base_link"/>
<param name="queue_size" type="int" value="40"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="wait_for_transform_duration" type="double" value="5.3"/>
<param name="Vis/MinInliers" type="string" value="12"/>
</node>
</group>
</launch>二台のandroidからストリーム配信を開始して実行します。
$ cd ~/ros_launch_files $ roslaunch stereo_rtsp_VB5500.launch $ source ~/ros_ws/devel/setup.bash $ roslaunch stereo_RTAB.launch
ここまでも試行錯誤しましたがエラーが発生しました。
[ WARN] [1599296766.785042898]: Could not get transform from /base_link to rtsp_frame after 5.300000 seconds (for stamp=1599296761.330938)! Error="canTransform: source_frame rtsp_frame does not exist.. canTransform returned after 5.30874 timeout was 5.3.". [ERROR] [1599296766.785148236]: TF of received image 0 at time 1599296761.330938s is not set! [ERROR] [1599296766.785192122]: Could not convert rgb/depth msgs! Aborting rtabmap update...
TF関連の問題っぽいです。とりあえずTFのチュートリアルをやりながら原因を探りたいと思います。
