ROS rtabmap_ros 自作 Stereo Camera

2. ROS 関連パッケージをインストール。
$ sudo apt-get install ros-melodic-uvc-camera
$ sudo apt-get install ros-melodic-image-*
$ sudo apt-get install ros-melodic-rqt-image-view

2.1 １個での実行
$ roscore

$ rosrun uvc_camera uvc_camera_node

$ rostopic list
/camera_info
/image_raw
...
が出てくれば、OK
image_view ノードによる確認。
$ rosrun image_view image_view image:=/image_raw

此処までは、以前試したので、すんなりと行きました。

3. Stereo(USB カメラ、2個) での場合。
Stereo の時は、どうするのか判らなかったのですが、
$ roscd uvc_camera
とすると、/opt/ros/melodic/share/uvc_camera とあったので
launch/ フォルダーを見ていたら、stereo_node.launch があったので、
$ roslaunch uvc_camera stereo_node.launch
/dev/video0 は、オープンOK で、
/dev/video1 のオープンでエラーになる。
USBカメラ1 だと /dev/video0,/dev/video1 がアサインされて、
USBカメラ2 だと /dev/video2,/dev/video3 がアサインされるので、
試しに、stereo_node.launch を、修正してみます。
<param name="left/device" type="string" value="/dev/video0" />
<!-- <param name="right/device" type="string" value="/dev/video1" /> -->
<param name="right/device" type="string" value="/dev/video2" />

これで、OK となりました。
確認は、Rviz でできます。
$ rviz
Add > By topic
/left/image_raw/Image
/right/image_raw/Image

4. 本題の、Stereo Handheld Mapping
を試してみます。

  3. Mapping mode
RealSense D415/D435 cameras: を真似て、

$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" left_image_topic:=/left/image_raw right_image_topic:=/right/image_raw left_camera_info_topic:=/left/camera_info right_camera_info_topic:=/right/camera_info stereo:=true frame_id:=base_link

ワーニングが出るみたい。
camera_link を出力してやらないといけないみたい。
[ WARN] [1630921124.202590786]: odometry: Could not get transform from camera_link to camera (stamp=1630921123.927631) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="canTransform: target_frame camera_link does not exist. canTransform: source_frame camera does not exist.. canTransform returned after 0.200775 timeout was 0.2."
試しに、下記を、実行したが、カメラは、２個あるので、カメラの中心位置か?
$ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 base_link camera_link 100
static_transform_publisher
まだ、ワーニングは、消えない。

5. Tutorials/Stereo Handheld Mapping を見直していたら、
  2. Bring-up the camera に書かれている事を、前準備として、行わないといけないとの事です。
ここが、完全に抜けちょりました。

  2.1 Bring-up example for a Bumblebee2 camera が、参考になるみたいです。
試行錯誤の結果、下記の様に、launch ファイルを作成しました。
その前に、rtabmap_ros パケージがUpdateされても影響がでないように、自分用のパッケージを作成します。
$ cd ~/catkin_ws/src
$ catkin_create_pkg rtabmap_ros_my rospy roscpp std_msgs
$ mkdir -p rtabmap_ros_my/launch
$ cd ..
$ catkin_make

ratbmap_ros_my/launch/rtabmap-nishi.launch
<launch> <include file="$(find uvc_camera)/launch/stereo_node.launch"> </include> <node pkg="tf" type="static_transform_publisher" name="odom_link" args="0 0 0 0 0 0 odom base_link 100" /> <!-- Rotate the camera frame. --> <arg name="pi/2" value="1.5707963267948966" /> <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" /> <node pkg="tf" type="static_transform_publisher" name="camera_base_link" args="$(arg optical_rotate) base_link stereo_camera 100" /> <node pkg="tf" type="static_transform_publisher" name="camera2_base_link" args="0 0 0 0 0 0 stereo_camera source_frame 100" /> <node pkg="tf" type="static_transform_publisher" name="camera3_base_link" args="0 0 0 0 0 0 source_frame camera 100" /> <!-- $ ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_image_proc --> <group ns="/stereo_camera" > <node name="stereo_image_proc" pkg="stereo_image_proc" type="stereo_image_proc"> <remap from="/stereo_camera/right/image_raw" to="/right/image_raw" /> <remap from="/stereo_camera/right/camera_info" to="/right/camera_info" /> <remap from="/stereo_camera/left/image_raw" to="/left/image_raw" /> <remap from="/stereo_camera/left/camera_info" to="/left/camera_info" /> </node>> </group> </launch>
tf 情報で、source_frame、camera のリンクが必要みたいです。
序に、odom と base_link の tf link も記述しました。

改めて、実行
$ roscore

$ roslaunch rtabmap_ros_my rtabmap-nishi.launch

この後、3. Mapping mode の RealSense D415/D435 cameras: を参考に実行します。
$ roslaunch rtabmap_ros stereo_mapping.launch rtabmap_args:="--delete_db_on_start" rviz:=true rtabmapviz:=true left_camera_info_topic:=/left/camera_info right_camera_info_topic:=/right/camera_info left_image_topic:=/stereo_camera/left/image_rect right_image_topic:=/stereo_camera/right/image_rect
少し、前にすすんだか!!
まだ、rviz で、画像が出てきません。
stereo_image_proc でワーニングがでているみたい。
[ERROR] [1631024071.443943013]: Rectified topic '/stereo_camera/left/image_rect_color' requested but camera publishing '/left/camera_info' is uncalibrated [ERROR] [1631024101.448408854]: Rectified topic '/stereo_camera/left/image_rect_color' requested but camera publishing '/left/camera_info' is uncalibrated [ERROR] [1631024131.449912928]: Rectified topic '/stereo_camera/right/image_rect' requested but camera publishing '/right/camera_info' is uncalibrated [ERROR] [1631024161.452321489]: Rectified topic '/stereo_camera/left/image_rect_color' requested but camera publishing '/left/camera_info' is uncalibrated
カメラのキャリブレーションをすれば良いのか?
下記を参考に、キャリブレーションを行います。
How to Calibrate a Stereo Camera
checkerboard (8x6) を印刷して、
印刷したチェックパターンの一辺が、2.5[cm] だったので、--square 0.025 にします。
$ 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
1) CAUBRATE がグリーンに変わるまで、チェックボードを動かします。
左右、上下、前後に移動、回す、斜めにする。
2) X、Y、Size、Skew(斜め) の下の緑色のバーが目安みたいです。十分長くなるまで、チェックボードを動かし続けます。
3) 十分であれば、CAUBRATE をクリックします。
4) Calibrating が終われば、COMMIT がグリーンに変わります。クリックすれば保存されますが、その際に、
書き込み許可のパーミッションにしておきます。
/opt/ros/melodic/share/uvc_camera (777)
5) 約、250サンプルで、Calibrating には、3.2 Ghz x 2 Core で、3から4時間程です。
Jetson Nano 2G だと.... PC でキャリブレーションして、結果をコピーしたほうが良いかも。
6) キャリブレーション を十分すれば、rtabmap_ros の stereo キャリブレーション不足のワーニングは、でなくなると思います。
但し、あとで判ったことですが、これだけでは、実測の距離と、Rviz 上の図面上の距離がかなりずれます。
これへの対応は、stereo_image_proc のパラメータの調節が必要になります。
stereo_image_proc で、距離の補正が、パラメータでできる様なので試してみます。
ROSで始めるロボティクス（9） ー ROSを使ったステレオカメラキャリブレーション
注) 昨今のブラウザーの所為か画像が、残念ながら見えません。
stereo_image_proc Tutorials ChoosingGoodStereoParameters 本家。
注2) 残念ながら、上記のパラメータ調整では、距離の補正は出来ないみたいです。やはりキャリブレーションをしっかりやります。
Skew は、100% 、Size は、60[%] 以上にします。
7) stereo_node.launch と、stereo_image_proc は、同じマシンで実行させます。Network を介すると、rtabmap_ros の
stereo キャリブレーション不足のワーニング がでたりします。

rviz 起動時にもエラーがでているみたい。
[ERROR] [1631026759.237310714]: PluginlibFactory: The plugin for class 'octomap_rviz_plugin/ColorOccupancyGrid' failed to load.
octomap_rviz_plugins が必要みたいだ。
$ sudo apt install ros-melodic-octomap-rviz-plugins

rtabmapviz では、何か表示されるようになりました。
ただし、表示内容がよくわからん。

起動ターミナルには、[ WARN ] や [ ERROR ] がいっぱい出てきます。

[ERROR] (2021-09-08 17:38:56.563) Rtabmap.cpp:1156::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 870 is ignored! [ INFO] [1631090336.565782900]: rtabmap (9): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0020s, Maps update=0.0000s pub=0.0000s (local map=2, WM=3) [ WARN] (2021-09-08 17:38:56.688) OdometryF2M.cpp:525::computeTransform() Registration failed: "Too low inliers after bundle adjustment: 2<20"

なんだかんだ言って、やっと出来たみたいぞね。
rtabmapviz 上で、3D Map 画面 で、マウスでドラッグして、向きや大きさを変えられるみたいぞね。

Rviz で、Add ->stereo_camera > points2 > PointClouds2 など表示すると、でてくるみたいだ。
/rtabmap/proj_map に、 2D マップが出ているみたい。試しに、保存してみます。
$ rosrun map_server map_saver -f ~/maps_rtab3 map:=/rtabmap/proj_map
保存出来ました。
であれば、これを、自作 Turtlebot3(foxbot_core3) に搭載してやれば、/odom 、/imu は、foxbot_core3 から適宜、更新されるので、
後は、 teleop_key で、適当に動かせば、2D Map(SLAM)が出来上がるのではないか!。
しかし、/rtabmap の下には、色んな map 情報が出ているみたいです。
2D map で作成されるのは、カメラの中心位置が、地面になるみたい。

固定カメラで、しばらく置いておくと、位置がドリフトしているみたい。
/odom トピックが書き変っているのか?
IMU 情報を与えてやれば、手持ちで動かせて、部屋の 2D Map が作れるのか。
/imu だけ与えれば、 /odom は、rtabmap_ros が更新するのか?

しかし、Stereo 画像のマッチングを、PC で処理するとやはり重たい。
これでは、SBC ( Jeston nano 2G ) 上では無理か?
やはり、 D435 等の、カメラ組み込みの、Depth 機能を使う必要があるみたいだ、

或いは、Stereo カメラの取り込みだけを、SBC で行って、それ以降は、PC で行うか?
将来的には、全て SBC に組み込まないといけないが。

うーーん。どうするか?