Multiple camera mapping

[jacklu333333]

Hi, I am tried using two cameras for rtabmap to have better odometry. I refer to the example launch file of "demo_two_kinects.launch." But the rtabmap did not work and it is not publishing the map topic.

Yet, the cameras I got are one Realsense L515 and one Astra camera. Therefore I remap all the input accordingly to the example file.

I check the input of odometry of RGDB  for both cameras they are both published by rgbd_odometry node.  

I am wondering what might be the cause? Is it because of the difference of frame_id for both cameras?

The following is my launch file.

<launch>
	<include file="$(find realsense2_camera)/launch/rs_camera.launch">
		<arg name="camera"	value="l515" />
		<arg name="depth_width"		value="640" />
		<arg name="depth_height"	value="480" />
		<arg name="color_width"		value="1280" />
		<arg name="color_height"	value="720" />

		<arg name="color_fps"		value="30" />
		<arg name="gyro_fps"		value="200" />
		<arg name="accel_fps"		value="200" />

		<arg name="enable_gyro"			value="true" />
		<arg name="enable_accel"		value="true" />
		<arg name="unite_imu_method"	value="linear_interpolation" />

		<arg name="initial_reset"	value="true" />

		<arg name="enable_confidence"	value="false" />

		<arg name="topic_odom_in"	value="/odometry/filtered" />
		<!-- <arg name="topic_odom_in"	value="/odom" /> -->
		<!-- <arg name="tf_publish_rate"	value="100" /> -->

		<arg name="enable_sync"	value="true" />
		<arg name="align_depth"	value="true" />
	</include>


 <env name="TURTLEBOT_3D_SENSOR" value="astra" />

  <include  file="$(find turtlebot_bringup)/launch/3dsensor.launch">
     <arg name="depth_registration" value="true"/>
  </include>
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
  
  <arg name="imu_topic"                default="/rtabmap/imu"/> 

   <!-- Choose visualization -->
   <arg name="rviz"       default="false" />
   <arg name="rtabmapviz" default="false" /> 
   

   <arg name="strategy"        default="1" />
   <arg name="feature"         default="6" />
   <arg name="nn"              default="3" />
   <arg name="max_depth"       default="4.0" />
   <arg name="min_inliers"     default="20" />
   <arg name="inlier_distance" default="0.02" />
   <arg name="local_map"       default="1000" />
   <arg name="odom_info_data"  default="true" />
   <arg name="wait_for_transform"  default="true" />
   
   <!-- sync rgb/depth images per camera -->
   <group ns="camera1">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync camera1_nodelet_manager">
      <remap from="rgb/image"         to="/l515/color/image_raw"/>
      <remap from="depth/image"       to="/l515/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info"   to="/l515/color/camera_info"/>
    </node>
   </group>
   <group ns="camera2">
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync camera2_nodelet_manager">
      <remap from="rgb/image"         to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"       to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info"   to="/camera/rgb/camera_info"/>
    </node>
   </group>
        
  <group ns="rtabmap">
  
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
	  
	  <param name="subscribe_rgbd"           type="bool"   value="true"/>
	  <param name="frame_id"                 type="string" value="odom"/>
	  <param name="rgbd_cameras"             type="int"    value="2"/>
	  <param name="wait_for_transform"       type="bool"   value="$(arg wait_for_transform)"/>
	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
	  <param name="OdomF2M/BundleAdjustment" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
	  <param name="Vis/EstimationType"      type="string" value="0"/> <!-- should be 0 for multi-cameras -->
	  <param name="Vis/FeatureType"         type="string" value="$(arg feature)"/> 
	  <param name="Vis/CorGuessWinSize"     type="string" value="0"/> 
	  <param name="Vis/CorNNType"           type="string" value="$(arg nn)"/>
	  <param name="Vis/MaxDepth"            type="string" value="$(arg max_depth)"/>  
	  <param name="Vis/MinInliers"          type="string" value="$(arg min_inliers)"/> 
	  <param name="Vis/InlierDistance"      type="string" value="$(arg inlier_distance)"/>       
      <param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/> 
      <param name="Odom/FillInfoData"        type="string" value="$(arg odom_info_data)"/>   
    </node>
    <param name="odom_frame_id"               type="string" value="odom"/>
    <remap from="odom"            to="/rtabmap/rgbd_odom"/>
    <remap from="imu"             to="$(arg imu_topic)"/>
    <param name="publish_tf"                  type="bool"   value="false"/>
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node if="false" name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"    type="int"    value="2"/>
	    <param name="frame_id"         type="string" value="base_footprint"/>
	    <param name="gen_scan"         type="bool"   value="true"/>
	    <param name="wait_for_transform" type="bool"   value="$(arg wait_for_transform)"/>
	    <param name="map_negative_poses_ignored" type="bool"   value="false"/>        <!-- refresh grid map even if we are not moving-->
	    <param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
      <remap from="imu"             to="$(arg imu_topic)"/>

      <param name="map_frame_id"         type="string" value="map"/>
      <param name="odom_frame_id"        type="string" value="odom"/>
      <param name="publish_tf"           type="bool"   value="true"/>

      <param name="odom_tf_angular_variance" type="double" value="0.001"/>
      <param name="odom_tf_linear_variance" type="double" value="0.001"/>

      <remap from="grid_map" to="/map"/>

      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>

      <param name="Grid/FromDepth"     type="string" value="false"/>
      <param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
      <param name="Vis/MinInliers"     type="string" value="10"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="rgbd_cameras"       type="int"    value="2"/>
      <param name="wait_for_transform"  type="bool"   value="$(arg wait_for_transform)"/>
    
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>

</launch>

[matlabbe]

If there are errors or warnings on the terminal it would help to find the problem.

Is /rtabmap/rgbd_odom published? If so rtabmap should be able to create a map, unless there is a warning telling that it is not able to sync input topics (because one is missing or something else).

[jacklu333333]

There is no log file that I can find in ros log folder, but I manage to solve it by swap the computer to another. The original one is Jetson nano, the replaced one is Laptop. I am not sure what is the cause of it but now it works.

Thanks.
Best Regards,
Jack Lu