3D LiDAR + Stereo + Odometry Problems

[loopclosure0]

Hello,

I'd like to use rtabmap_ros with a 3D LiDAR, ZED Stereo Camera, and an odometry source from that ZED stereo camera. I'm able to create maps with the 3D LiDAR + odometry, and I'm able to create maps with the ZED + Odometry, but I'm not able to create maps with all three at once.

I'm following the post from github (https://github.com/introlab/rtabmap_ros/issues/303), which uses an Ouster LiDAR which is the same as the one I'm using. Unfortunately I can't get it to work.  I get the below error, and the lidar scans appear, but the stereo pointcloud doesn't show up, the white line for odometry doesn't appear, and no new nodes are created.

"[ERROR] (2019-08-26 18:19:45.982) Memory.cpp:4016::createSignature() Camera calibration not valid, calibrate your camera!
[ERROR] (2019-08-26 18:19:45.982) Memory.cpp:788::update() Failed to create a signature...

Do you mind taking a look at my launch file, to see if I'm doing anything wrong? RTAB-Map is really powerful, and I've gotten great results just the LiDAR alone, but I'd like to add stereo so I can do loop closure & localization on a pre-built map.

github_stereo_lidar.launch

[matlabbe]

Hi,

I will recopy the launch file here with changes I think may help. If not working, copy/paste the warnings in a new post.

<launch>
  

    <arg name="frame_id" default="os1_lidar"/>
    <arg name="rtabmapviz"    default="true"/>
    <arg name="scan_20_hz"    default="false"/>
    <arg name="use_sim_time"  default="true"/>
    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
    

    <arg name="left_image_topic"  default="/front_zed/left/image_rect_color"/>
    <arg name="right_image_topic"  default="/front_zed/right/image_rect_color"/>
    <arg name="left_camera_info_topic"  default="/front_zed/left/camera_info"/>
    <arg name="right_camera_info_topic"  default="/front_zed/left/camera_info"/>

    <group ns="rtabmap">

        <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>

      <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     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="false"/>
        <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="subscribe_rgbd"       type="bool" value="true"/>
        <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_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="true"/>
        
        <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="subscribe_rgbd" type="bool" value="true"/>
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="approx_sync" type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
        <param name="subscribe_depth" value="false"/>
      </node>
  </group>

</launch>

cheers,
Mathieu