search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
974 stars 557 forks source link

Does 3d Lidar work with a stereo setup #303

Closed zarmomin closed 5 years ago

zarmomin commented 5 years ago

The paper suggests that RGBD/Stereo Cameras can be interchanged seamlessly. However, when I run the "https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/test_ouster.launch" but swap the RGBD for a stereo subscription, rtabmap won't subscribe to the lidar data. The latest scan is shown in the rtabmapviz node however. Did I simply miss a parameter?

zarmomin commented 5 years ago

This is the relevant part of the launchfile:

<group ns="rtabmap">
      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>

        <remap from="imu" to="/os1_cloud_node/imu/data"/>
        <param name="guess_frame_id"   type="string" value="$(arg frame_id)"/>
        <param name="wait_imu_to_init" type="bool" value="true"/>
        <param name="Reg/Strategy"                   type="string" value="1"/>

        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="false"/>
        <param name="Icp/Iterations"          type="string" value="10"/>
        <param name="Icp/VoxelSize"           type="string" value="0.2"/>
        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
        <param name="Icp/Epsilon"             type="string" value="0.001"/>
        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>

        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
        <param name="Odom/Strategy"              type="string" value="1"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
      </node>

      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan" type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="true"/>
        <param name="subscribe_stereo" type="bool" value="true"/>
            <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
            <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
            <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
            <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
            <param name="subscribe_depth" value="false"/>
        <remap from="scan_cloud" to="/os1_cloud_node/points"/>

        <!-- RTAB-Map's parameters -->
        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <!--param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/-->
        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
        <param name="Mem/STMSize"                    type="string" value="30"/>
        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->

        <param name="Reg/Strategy"                   type="string" value="1"/>
        <param name="Grid/CellSize"                  type="string" value="0.1"/>
        <param name="Grid/RangeMax"                  type="string" value="20"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>

        <!-- ICP parameters -->
        <param name="Icp/VoxelSize"                  type="string" value="0.2"/>
        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
        <param name="Icp/PointToPlane"               type="string" value="false"/>
        <param name="Icp/Iterations"                 type="string" value="10"/>
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
        <param name="Icp/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="0.4"/>
      </node>

      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync" type="bool" value="true"/>
        <param name="subscribe_scan" type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
        <param name="subscribe_stereo" type="bool" value="true"/>
            <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
            <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
            <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
            <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
            <param name="subscribe_depth" value="false"/>
      </node>
  </group>
matlabbe commented 5 years ago

This is not implemented for stereo to force that we synchronize stereo topics together into a RGBDImage topic prior to rtabmap node. This extra step is required only when subscribing also to a scan topic because stereo images should be exactly synchronized before doing the approximate synchronization with the lidar.

In your launch file, add the following:

<node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="screen">
          <remap from="left/image_rect"   to="$(arg left_image_topic)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
          <param name="approx_sync"     type="bool"   value="false"/>
</node>

Then for rtabmap and rtabmapviz nodes, add: <param name="subscribe_rgbd" type="bool" value="true"/>

This will make them subscribing to /rtabmap/rgbd_image and /os1_cloud_node/points.

EDIT: note that for RGB-D topics, it is recommended to do the same (synchronizing rgb and depth topics before rtabmap) with rgbd_sync node instead.

cheers, Mathieu

zarmomin commented 5 years ago

Hi Mathieu, thanks that did the trick! Cheers, Nico