ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

1) In your rqt_graph, the left image and camera info are not shown, are they correctly connected to stereo_img_proc, stereo_odometry and rtabmap? If you are holding your camera, you may do something like a mix of test_stereo_odometry.launch and demo_stereo_outdoor.launch as the Stereo B configuration:

<launch>
<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" />  

<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="left/image_rect"/>
        <remap from="right/image_rect"      to="right/image_rect"/>
        <remap from="left/camera_info"      to="left/camera_info"/>
        <remap from="right/camera_info"     to="right/camera_info"/>

        <param name="frame_id" type="string" value="base_link"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="approx_sync" type="bool" value="false"/>
        <param name="queue_size" type="int" value="5"/>

        <param name="Odom/InlierDistance" type="string" value="0.1"/>
        <param name="Odom/MinInliers" type="string" value="10"/>
        <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
        <param name="Odom/MaxDepth" type="string" value="10"/>

        <param name="GFTT/MaxCorners" type="string" value="500"/>
        <param name="GFTT/MinDistance" type="string" value="5"/>
    </node>     
</group>

<group ns="rtabmap">   
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
     <param name="frame_id" type="string" value="base_link"/>
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_depth" type="bool" value="false"/>

     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>

     <remap from="odom" to="/stereo_camera/odom"/>

     <param name="queue_size" type="int" value="30"/>

     <!-- RTAB-Map's parameters -->
     <param name="Rtabmap/TimeThr" type="string" value="700"/>
     <param name="Rtabmap/DetectionRate" type="string" value="1"/>

     <param name="Kp/WordsPerImage" type="string" value="200"/>
     <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
     <param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
     <param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->

     <param name="SURF/HessianThreshold" type="string" value="1000"/>

     <param name="LccBow/MaxDepth" type="string" value="5"/>
     <param name="LccBow/MinInliers" type="string" value="10"/>
     <param name="LccBow/InlierDistance" type="string" value="0.02"/>

     <param name="LccReextract/Activated" type="string" value="true"/>
     <param name="LccReextract/MaxWords" type="string" value="500"/>
  </node>

  <!-- Visualisation RTAB-Map -->
  <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_odom_info" type="bool" value="true"/>
     <param name="queue_size" type="int" value="10"/>
     <param name="frame_id" type="string" value="base_link"/>
     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
     <remap from="odom_info" to="/stereo_camera/odom_info"/>
     <remap from="odom" to="/stereo_camera/odom"/>
  </node>

 </group>
 </launch>

Here some screenshots of the connections: image description image description image description

Note that stereo_odometry and rtabmap nodes assume by default that left and right images' (and respective camera infos) timestamps are exact. If your cameras are not synchronized, you can set "stereo_approx_sync" to true for the rtabmap node and "approx_sync" to true for the stereo_odometry node.

2) the Tf looks ok. The only TF required is a transform from /base_link to the camera link set in the image messages. In RVIZ, make sure you set your fixed frame id to /map.

cheers

1) In your rqt_graph, the left image and camera info are not shown, are they correctly connected to stereo_img_proc, stereo_odometry and rtabmap? If you are holding your camera, you may do something like a mix of test_stereo_odometry.launch and demo_stereo_outdoor.launch as the Stereo B configuration:

<launch>
<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" />  

<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="left/image_rect"/>
        <remap from="right/image_rect"      to="right/image_rect"/>
        <remap from="left/camera_info"      to="left/camera_info"/>
        <remap from="right/camera_info"     to="right/camera_info"/>

        <param name="frame_id" type="string" value="base_link"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="approx_sync" type="bool" value="false"/>
        <param name="queue_size" type="int" value="5"/>

        <param name="Odom/InlierDistance" type="string" value="0.1"/>
        <param name="Odom/MinInliers" type="string" value="10"/>
        <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
        <param name="Odom/MaxDepth" type="string" value="10"/>

        <param name="GFTT/MaxCorners" type="string" value="500"/>
        <param name="GFTT/MinDistance" type="string" value="5"/>
    </node>     
</group>

<group ns="rtabmap">   
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
     <param name="frame_id" type="string" value="base_link"/>
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_depth" type="bool" value="false"/>

     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>

     <remap from="odom" to="/stereo_camera/odom"/>

     <param name="queue_size" type="int" value="30"/>

     <!-- RTAB-Map's parameters -->
     <param name="Rtabmap/TimeThr" type="string" value="700"/>
     <param name="Rtabmap/DetectionRate" type="string" value="1"/>

     <param name="Kp/WordsPerImage" type="string" value="200"/>
     <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
     <param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
     <param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->

     <param name="SURF/HessianThreshold" type="string" value="1000"/>

     <param name="LccBow/MaxDepth" type="string" value="5"/>
     <param name="LccBow/MinInliers" type="string" value="10"/>
     <param name="LccBow/InlierDistance" type="string" value="0.02"/>

     <param name="LccReextract/Activated" type="string" value="true"/>
     <param name="LccReextract/MaxWords" type="string" value="500"/>
  </node>

  <!-- Visualisation RTAB-Map -->
  <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
     <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_odom_info" type="bool" value="true"/>
     <param name="queue_size" type="int" value="10"/>
     <param name="frame_id" type="string" value="base_link"/>
     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
     <remap from="odom_info" to="/stereo_camera/odom_info"/>
     <remap from="odom" to="/stereo_camera/odom"/>
  </node>

 </group>
 </launch>

Here some screenshots of the connections: image description image description image description

Note that stereo_odometry and rtabmap nodes assume by default that left and right images' (and respective camera infos) timestamps are exact. If your cameras are not synchronized, you can set "stereo_approx_sync" to true for the rtabmap node and "approx_sync" to true for the stereo_odometry node.

2) the Tf looks ok. The only TF required is a transform from /base_link to the camera link set in the image messages. In RVIZ, make sure you set your fixed frame id to /map.

[UPDATE]

a) The odometry is working, but the quality is quite low. You can try the parameters of this stereo tutorial. In ROS, copy the parameters under stereo_odometry node in your launch file.

b) rtabmap node doesn't seem to be correctly connected, so without inputs, there is no output. Make sure to have the same connections as the second rqt_graph that I have shown above for rtabmap node: /camera/left/camera_info, /camera/right/camera_info, /camera/left/image_rect_color, /camera/right/image_rect and /camera/odom.

cheers