Official RTAB-Map Forum

L515+T265 stops mapping after a few frames

classic Classic list List threaded Threaded
♦
♦
 11 messages Options
zyhzyh0333
Reply | Threaded
Open this post in threaded view
♦
♦
|

L515+T265 stops mapping after a few frames

zyhzyh0333
This post was updated on .
Hi, I'm now working on combining L515 and T265 for 3D reconstruction (with ROS), and the rtabmap stops recording after a few frames. Here is the image that rtabmap stop stops at frame ID = [15] I follow the instruction from http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping Here is my launch file rs_l515_and_t265.launch (for realsense2): 
<launch>
  <arg name="device_type_camera1"    		default="t265"/>
  <arg name="device_type_camera2"    		default="l515"/>		<!-- Note: using regular expression. match D435, D435i, D415... -->
  <arg name="serial_no_camera1"    			default=""/>
  <arg name="serial_no_camera2"    			default=""/>
  <arg name="camera1"              			default="t265"/>
  <arg name="camera2"              			default="l515"/>
  <arg name="tf_prefix_camera1"         default="$(arg camera1)"/>
  <arg name="tf_prefix_camera2"         default="$(arg camera2)"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="enable_fisheye"            default="false"/>
  <arg name="color_width"               default="640"/>
  <arg name="color_height"              default="480"/>
  <arg name="depth_width"               default="640"/>
  <arg name="depth_height"              default="480"/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>

  <group ns="$(arg camera1)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="device_type"           value="$(arg device_type_camera1)"/>
      <arg name="serial_no"             value="$(arg serial_no_camera1)"/>
      <arg name="tf_prefix"         		value="$(arg tf_prefix_camera1)"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
      <arg name="enable_fisheye1"       value="$(arg enable_fisheye)"/>
      <arg name="enable_fisheye2"       value="$(arg enable_fisheye)"/>
      <arg name="topic_odom_in"         value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"       value="$(arg calib_odom_file)"/>
      <arg name="enable_pose"           value="true"/>
    </include>
  </group>

  <group ns="$(arg camera2)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="device_type"           value="$(arg device_type_camera2)"/>
      <arg name="serial_no"             value="$(arg serial_no_camera2)"/>
      <arg name="tf_prefix"		          value="$(arg tf_prefix_camera2)"/>
      <arg name="initial_reset"         value="$(arg initial_reset)"/>
      <arg name="align_depth"           value="true"/>
      <arg name="filters"               value="pointcloud"/>
      <arg name="color_width"           value="$(arg color_width)"/>
      <arg name="color_height"          value="$(arg color_height)"/>
      <arg name="depth_width"           value="$(arg depth_width)"/>
      <arg name="depth_height"          value="$(arg depth_height)"/>
      <arg name="clip_distance"         value="$(arg clip_distance)"/>
    </include>
  </group>
  <node pkg="tf" type="static_transform_publisher" name="t265_to_l515" args="0 0 0 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/>
</launch>
And my command to start rtabmap is: 
$ roslaunch realsense2_camera rs_l515_and_t265.launch

$ roslaunch rtabmap_ros rtabmap_265.launch \
   args:="-d --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
   odom_topic:=/t265/odom/sample \
   frame_id:=t265_link \
   rgbd_sync:=true \
   depth_topic:=/l515/aligned_depth_to_color/image_raw \
   rgb_topic:=/l515/color/image_raw \
   camera_info_topic:=/l515/color/camera_info \
   approx_rgbd_sync:=false \
   appox_sync:=true \
   visual_odometry:=false
In file "ros rtabmap_265.launch", I follow another post: http://official-rtab-map-forum.206.s1.nabble.com/Has-anyone-got-l515-t265-working-td8193.html#a8278 and I've changed "subscribe_odom_info" to false. Here's the whole launch file: 
<!-- -->
<launch>
  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For stereo:=false
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between depth and stereo, set both to false to do only scan -->      
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>

  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>         
   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>
  
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
  
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>

  <!-- Nodes -->
  <group ns="$(arg namespace)">
     
    <!-- relays -->
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
          <param name="depth_scale"     type="double" value="$(arg rgbd_depth_scale)"/>
          <param name="decimation"     type="double" value="$(arg rgbd_decimation)"/>
        </node>
      </group>
    </group>
    <group if="$(arg stereo)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
          <remap from="left/image_rect"   to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
          <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="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
        </node>
      </group>
    </group>
    
    <group unless="$(arg rgbd_sync)">
       <group if="$(arg subscribe_rgbd)">
         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
           <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
           <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
           <param if="$(arg compressed)" name="uncompress" value="true"/>
         </node>
      </group>
    </group>

    <!-- Visual odometry -->
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">
      
        <!-- RGB-D Odometry -->
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>
      
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
          <param name="config_path"                 type="string" value="$(arg cfg)"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
        </node>

        <!-- Stereo Odometry -->
        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
          <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="$(arg rgbd_topic_relay)"/>
          <remap from="odom"                   to="$(arg odom_topic)"/>
          <remap from="imu"                    to="$(arg imu_topic)"/>
      
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
          <param name="config_path"                 type="string" value="$(arg cfg)"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
        </node>
      </group>
    </group>
    
    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
      <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
      <param name="config_path"                 type="string" value="$(arg cfg)"/>
      <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
      <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
      <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
      <param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
      <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
      <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
    </node>
    
    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>

      <remap from="odom"            to="$(arg odom_topic)"/>
      <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
      <param name="fixed_frame_id"  type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
      <param name="voxel_size"      type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
      <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
      <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="false"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="false"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>      
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param     if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="false"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="false"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
      
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>
I've also followed this issue: https://github.com/introlab/rtabmap_ros/issues/555 and checked the topics as follow, 
rostopic hz /t265/odom/sample \
  /l515/aligned_depth_to_color/image_raw \
  /l515/color/image_raw \
  /l515/color/camera_info
showing Thanks in advance for your help. zyhzyh0333
matlabbe
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

matlabbe
Administrator
Hi,

rtabmapviz is in Pause mode (button Pause is clicked), which pauses rtabmap process. Did you pause after it stopped?

Which realsense sdk version are you using? A problem I've seen with the ros wrapper is that the clock between the cameras may drift between each other, to eventually make rtabmap (ros approximate synchronizer) not able to synchronize topics as the delay between their stamps is too large. You may check for options to start the cameras with global computer time (frames are restamped with computer clock at arrival). To see if this is the problem, I open two terminals, then do: 
rostopic echo /265/odom/sample/header/stamp
rostopic echo /l515/color/camera_info/header/stamp
and check after some time if the latest stamps from both terminal have drifted from each other.

cheers,
Mathieu
zyhzyh0333
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

zyhzyh0333
Dear Mathieu,

Thank you for your reply.

However, it stops itself after several frames.

To figure out the problem, I used a D435i with T265. I changed all the "l515" in realsense launch file and rtabmap ros arguments to "d435i", and everything works properly.

Then, I've used rosbag to record several topics for D435i and T265, it works fine. When I recording the bag, I also opened the rtabmap-ros to see if it was working properly.

However, when I switch back to L515 and T265, the rosbag recording would show a warning message about "[ WARN] [1625201375.003035664]: rosbag record buffer exceeded. Dropping oldest queued message." And this is exactly the same time when rtabmap stoped recording.
This doesn't happen with D435i and T265 case. I've searched for this case, and I add "-b 4096" to rosbag, there's no warning when recoding the bag, but after I play back the bag, the rtabmap won't show anything.

I think it's because the buffer exceeds for rtabmap? Maybe the same thing happens for rtabmap and rosbag....Is there any way to enlarge the buffer size of rtabmap~
matlabbe
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

matlabbe
Administrator
rtabmapviz will pause automatically only if it detects an error on its side, which should show up the UI console with the error displayed.

You may check these issues:
https://github.com/IntelRealSense/realsense-ros/issues/1965
https://github.com/stereolabs/zed-ros-wrapper/issues/333
https://github.com/stereolabs/zed-ros-wrapper/issues/345

If the "Pause" button is not toggled and rtabmap is not processing anything, it may have difficulty to synchronize the topics if they are badly recorded or not published at the same time.

You may try to record lower resolution L515 images. Maybe the difference is that default D435i image resolution is lower than the L515, then your hard-drive/computer has difficulty to record all L515 data in real-time while with D435i images it is fine.

To help with topic synchronization, you can increase "queue_size" parameter on rtabmap side. You can also check when you replay a rosbag with "rostopic hz" on all image topics to see if there is a difference than when streaming directly from the camera.
zyhzyh0333
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

zyhzyh0333
Hi~ Thank you very much for your reply~ It just stops without showing an error. I've recorded this and you can check the screen recording via https://www.youtube.com/watch?v=PQr8_KULZL4 Besides, I've adjusted the "queue_size" from 10 to 30, but there's still the same problem. And I'm not sure whether the default resolution of L515 is modified by the launch file: 
<launch>
  <arg name="device_type_camera1"    		default="t265"/>
  <arg name="device_type_camera2"    		default="l515"/>		<!-- Note: using regular expression. match D435, D435i, D415... -->
  <arg name="serial_no_camera1"    			default=""/>
  <arg name="serial_no_camera2"    			default=""/>
  <arg name="camera1"              			default="t265"/>
  <arg name="camera2"              			default="l515"/>
  <arg name="tf_prefix_camera1"         default="$(arg camera1)"/>
  <arg name="tf_prefix_camera2"         default="$(arg camera2)"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="enable_fisheye"            default="false"/>
  <arg name="color_width"               default="640"/>
  <arg name="color_height"              default="480"/>
  <arg name="depth_width"               default="640"/>
  <arg name="depth_height"              default="480"/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>

  <rosparam>
And this is the same setting I used for d435i, simply changed the camera name. Thank you in advance for your patience~
matlabbe
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

matlabbe
Administrator
Hi,

You could try with odometry from TF instead: 
roslaunch rtabmap_ros rtabmap_265.launch \
   args:="-d --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
   frame_id:=t265_link \
   rgbd_sync:=true \
   depth_topic:=/l515/aligned_depth_to_color/image_raw \
   rgb_topic:=/l515/color/image_raw \
   camera_info_topic:=/l515/color/camera_info \
   approx_rgbd_sync:=false \
   appox_sync:=true \
   visual_odometry:=false \
   odom_frame_id:=t265_odom_frame \
   odom_tf_angular_variance:= 0.001 \
   odom_tf_linear_variance:=0.0001
to avoid large sync queue as the odom is published at 200 Hz. However, by opening two terminals with: 
rostopic echo /rtabmap/rgbd_image/header
rostopic echo /t265/odom/sample/header
I can see that L515 stamps seem drifting against t265 stamps, I had 2 seconds difference after 5 min. I stop investigated this issue because I have a bigger issue on my side, see below.

I tried with my cameras (t265+L515) and I have issues with very low FPS with L515 topics. I don't have 30 Hz on all topics like you. I am using realsense 2.50 SDK (this commit) with latest firmware, not sure if it is the issue on my side. However, with rtabmap standalone with dual mode T265+L515, I have full 30 Hz stream and no lag. The realsense-viewer is also showing data at 30 Hz. I think the issue is coming from realsense ros package (version I am using). Here just by launching your t265+L515 launch (rtabmap is not started):

Sometimes I get all l515 topics at 16 Hz, but never 30 hz (the realsense nodelet is using more than 100% CPU, what is the CPU usage on your side?). With D435i, I get all topics at 30 Hz as you observed. My computer is a XPS15-7590 with Ubuntu 20.04.
cdb0y511
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

cdb0y511
Hi, matlabbe.
I have L515 and T265 too.
I want to do something similar like zyhzyh0333.
But I have some questions.
1. Does T265 really help? I find L515 is quite good for a small room. Maybe T265 will help in a large scene?
2. How to calibrate T265 and L515? Namely, how to get the TF? I find it difficult for calibration cause t265 is a fisheye camera. 3D print a specified gear? I am failed to use this (https://github.com/puzzlepaint/camera_calibration) to calculate the transform matrix cause the matching is not very reliable.
Ps. Frames drops come for many reasons, like the USB hub or CPU usage; if you suffer from high CPU usage, maybe building SDK with CUDA option will help.
I am looking forward to your progress.
Thanks,
cdb0y511
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

cdb0y511
In reply to this post by matlabbe
Hi, The standalone works fine with L515 + T265.
I still wonder whether T265 increases the estimate accuracy.
1. for ros version, I still have many errors, like the I get "/catkin_ws2/src/rtabmap_ros/src/MsgConversion.cpp:1522:7: error: ‘rtabmap_ros::OdomInfo’ {aka ‘struct rtabmap_ros::OdomInfo_<std::allocator<void> >’} has no member named ‘localBundleIds’; did you mean ‘localBundleTime’?
 1522 |   msg.localBundleIds.push_back(iter->first);
" when building from source.
2. I wish I could use the API directly, but the L515 + T265 seems more complicated than the cpp RGB-D example. Could you give a new CPP example for L515 + T265?
Thanks,
cdb0y511
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

cdb0y511
In reply to this post by matlabbe
At last, I think we may be on the same page. As your suggestion, it works finally.
The default lib from ros neotic work smoothly(pcl-ros(1.10) and opencv(4.2) with vtk(7.2),ubuntu 20.04,SDK 2.50,latest firewaremare).
But some adjustments need to be done to improve performance,
1. the tf t265_to_l515 is the extrinsic calibration from rtabmap?
   <node pkg="tf" type="static_transform_publisher" name="t265_to_l515" args="0 0 0 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/> is the "0 0 0 0 0 0 " should be extrinsic calibration from rtabmap?
2. I wonder why the ros version is worse than the standalone?
Any advice will be grateful.
Thanks,
zyhzyh0333
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

zyhzyh0333
In reply to this post by matlabbe
Thank you very much for your reply!

It works for me when using the odometry from TF instead.

I'll keep testing.
cdb0y511
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: L515+T265 stops mapping after a few frames

cdb0y511
I am glad you have made some progress, but I wonder if the TF has gone through all processes. I mean the TF node outputs are related with t265_to_l515, t265 odometry, ICP odometry(if ICP odometry set true), loop optimization(maybe?).
My problem is the ros version can not match the standalone performance.
Does the standalone ICP (libpointmatcher) still work when the odometry sensor is set to t265 (ICP help to reduce the estimation error, I think the ICP is necessary when the extrinsic calibration is not so accurate)?
Do you fit the extrinsic calibration from standalone to t265_to_l515 TF?
Free forum by Nabble Edit this page