Mapping performance and hallway problems with Kinect 2

[Wezza]

Hi!

I work with Ubuntu 16.04, ROS Kinetic and a Kinect 2 (for XBox One) sensor on an self-made mobile robot without odometry, that's the reason why I have to use visual_odometry.
I am using remote mapping (http://wiki.ros.org/rtabmap_ros/Tutorials/RemoteMapping) to transport the data from a laptop attached to the robot to my PC via WLAN. It is a little bit laggy, but generally it works (not even close to this... https://www.youtube.com/watch?v=Bh8WZsU4YC8).
RTABMAP works well in my lab with different kinds of objects. But now i have a problem. I want the robot to leave the lab and enter the hallway to build a map from the entire floor. The hallway (20m x 3m) is relatively empty with white walls. The Kinect therefore recognizes only very few points for visual_odometry and my odomtery is lost (red screen).
-


My final goal is to create a big map from several small maps.

My questions are:
1.) Is there a way to map the hallway with the Kinect sensor?
2.) Is there a way to improve the remote mapping performance?

Technical Details laptop: Core i7-4500U CPU @ 1.80 GHz x 4, 8GB RAM, Intel integrated graphics card, SSD.
Technical details PC: Core i5-3470 CPU @ 3.20 GHz x 4, 8GB RAM, GeForce 8400GS/PCIe/SSE2, HDD.

And this is my launchfile for rtabmap_ros:

<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 RGB-D and stereo -->      
  <arg name="stereo"          default="false"/>
 
  <!-- 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"/>		<!-- Default is 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="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  <arg name="output"                  default="screen"/>        <!--Control node output (screen or log)-->
  
  <!-- 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="true"/>         
   
  <!-- 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" />
  
  <!-- 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="subscribe_rgbd"          default="false"/>
  <arg name="rgbd_topic"              default="/camera/rgbd_image" />
  
  <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"/>  <!-- Common types: compressed, theora (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="scan_normal_k"           default="0"/>
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"               default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="vo_frame_id"              default="odom"/>
  <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="$(arg rtabmap_args)"/>
  <arg name="odom_sensor_sync"         default="false"/>
  
  
  <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) -->
  
  <!-- 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)"/>

  <!-- Changed Parameters for better performance -->
  
  <!-- ODOMETRY MAIN ARGUMENTS: 
        -"strategy"        : Strategy: Frame-to-Map 1=Frame-To-Frame
        -"feature"         : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
        -"nn"              : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE 
                             Set to 1 for float descriptor like SIFT/SURF                  
                             Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK  
        -"max_depth"       : Maximum features depth (m)  
        -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)  
        -"inlier_distance" : RANSAC maximum inliers distance (m)  
        -"local_map"       : Local map size: number of unique features to keep track 
        -"odom_info_data"  : Fill odometry info messages with inliers/outliers data.
    -->
   <!-- Default 0 Comment: -->
   <arg name="strategy" default="1" />
   <!-- Default 6 Comment: -->
   <arg name="feature" default="6" />
   <!-- Default 3 Comment: -->
   <arg name="nn" default="3" />
   <!-- Default 10 Comment: -->
   <arg name="max_depth" default="10" />
   <!-- Default 20 Comment: -->
   <arg name="min_inliers" default="20" />
   <!-- Default 0.02 Comment: -->
   <arg name="inlier_distance" default="0.02" />
   <!-- Default 1000 Comment: -->
   <arg name="local_map" default="1000" />
   <!-- Default 1000 Comment: -->
   <arg name="gftt_max_corners" default="1000" />
   <!-- Default 7 Comment: -->
   <arg name="gftt_min_distance" default="7" />

  <!-- End of changed Parameters for better performance -->

  <!-- Nodes -->
  <group ns="$(arg namespace)">
  
    <!-- RGB-D Odometry -->
    <group unless="$(arg stereo)">
      <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 if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" 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)"/>

	<!-- Changed Parameters for better performance -->

        <!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) -->
        <param name="Odom/Strategy" value="1"/>
        <!-- Correspondences: 0=Features Matching, 1=Optical Flow -->
        <param name="Vis/CorType" value="0"/>
        <!-- maximum features map size, default 2000 -->
        <param name="OdomF2M/MaxSize" type="string" value="1000"/> 
        <!-- maximum features extracted by image, default 1000 -->
        <param name="Vis/MaxFeatures" type="string" value="500"/>
	  
	<!-- end of changed Parameters for better performance -->        

	<param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
        <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="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)"/>
      </node>
    </group>
    
    <!-- Stereo Odometry -->
    <group if="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed 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="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" 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)"/>
	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
        <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="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)"/>
      </node>
    </group>
  
    <!-- 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="true"/>
      <param unless="$(arg stereo)" name="subscribe_rgbd"  type="bool" value="$(arg subscribe_rgbd)"/>
      <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_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <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="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="$(arg approx_sync)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_normal_k"        type="int" value="$(arg scan_normal_k)"/>

	<!-- Changed Parameters for better performance -->
	
	<param name="Reg/Force3DoF"    value="true"/>
        <param name="Optimizer/Slam2D" value="true"/>
	<param name="grid_size" type="double" value="30"/> <!-- 30 meters wide -->
	<param name="proj_max_height" value="2.0"/>
      
	<!-- end of changed Parameters for better performance -->

      <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)"/>
	
      <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 from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_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="true"/>
      <param unless="$(arg stereo)" name="subscribe_rgbd"  type="bool" value="$(arg subscribe_rgbd)"/>
      <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_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <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="$(arg approx_sync)"/>
    
      <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)"/>
      
      <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 from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" 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">
    <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap from="right/image"       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="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="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>
</launch>

[matlabbe]

Hi Wezza,

1) Unfortunately, environments like that cannot be mapped robustly using a single kinect without other kind of odometry. At our lab, we have the same problem, it is why we are using relatively accurate wheel odometry combined with a lidar to navigate in feature-less corridors for most of our experiments. If you can afford a lidar, I highly recommend it. If you can change the environment, adding posters on the wall would help to have more visual features to track. Note that I also tried Google Tango (which is a quite accurate visual inertial odometry approach) in such environements, and the results are a lot worst (1 to 2 meters drift when the fisheye sees only a white wall) than wheel odometry alone (even without a lidar refinement). Many of the visual-only SLAM limitations are discussed in the following paper where we used wheel odometry and only a kinect: Cheap or Robust? The practical realization of self-driving wheelchair technology.

2) You may want to increase the framerate of the published images. For visual odometry, at least 10 Hz is preferred, or even more if your wireless network can support as much as data. Beware that WiFi connection would not be as good everywhere in the building, in particular the robot has to switch between wifi spots while navigating. In that last case, having odometry and mapping running onboard would be preferred. Make sure your computers are synchronized, that could fix some difficulties to synchronize topics and TF on the desktop side.

cheers,
Mathieu

[Wezza]

First of all, thank you for the detailed answer and the abstract!

1.) Okay, that's what I was afraid of. Maybe I can add other sensors at a later date.

2.) I changed the rate in the "throttle" launchfile but i can't see any difference. Thats strange. I tried rate 1, 5, 10, 20, 30, 100. No difference in bandwidth (~495 kiB/s, qHD) and mapping quality/speed. The compression works, but rate has no influence. Or do I get the wrong idea about rate? I thought it was the parameter for the refresh/frame rate. My launchfile looks like this:

<launch>
  <arg name="rate"  default="20"/>
  <arg name="decimation"  default="1"/> <!-- Reduce the image size, e.g., 2 means "width/2 x height/2". -->
  <arg name="resolution" default="qhd" />
  <arg name="approx_sync" default="false" /> <!-- kinect2_bridge has synchronized images -->

  <!-- Use kinect2 nodelet manager -->
  <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle kinect2" output="screen">
      <param name="rate" type="double" value="$(arg rate)"/>
      <param name="decimation" type="int" value="$(arg decimation)"/>
      <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>

      <remap from="rgb/image_in"       to="/kinect2/$(arg resolution)/image_color_rect"/>
      <remap from="depth/image_in"     to="/kinect2/$(arg resolution)/image_depth_rect"/>
      <remap from="rgb/camera_info_in" to="/kinect2/$(arg resolution)/camera_info"/>

      <remap from="rgb/image_out"       to="/kinect2/data_throttled_image"/>
      <remap from="depth/image_out"     to="/kinect2/data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="/kinect2/data_throttled_camera_info"/>
  </node>    
</launch>

If I run mapping and odometry on the robot, how can I monitor the process? As far as I know, I need at least one image (odometry) to monitor the mapping. I was not able to subscribe to a matching topic which shows me the odometry video.

By synchronizing the computer, you mean a solution such as ntpdate?

Best regards,
Wezza

[matlabbe]

2) You may try "$ rostopic hz /kinect2/data_throttled_image/compressed" on remote computer to see what is the actual rate received.

You can use rtabmapviz part of your launch file or RVIZ on remote computer to see mapping results. You can remove subscribe_depth from rtabmapviz node (without rgb/depth/camera_info topics) to avoid subscribing to image data, thus saving bandwidth.

Yes, ntpdate.

cheers,
Mathieu

[Wezza]

Hello again!

I have used ntp to synchronize the computers, i hope that's okay too.

I set rate=10, the output for

"rostopic hz /kinect2/data_throttled_camera_info" average rate: 8.405
	min: 0.060s max: 0.200s std dev: 0.01963s window: 68

"rostopic hz /kinect2/data_throttled_image/compressed" average rate: 8.546
	min: 0.085s max: 0.145s std dev: 0.01778s window: 51

"rostopic hz /kinect2/data_throttled_image_depth/compressedDepth" average rate: 0.585
	min: 1.327s max: 4.592s std dev: 0.82700s window: 20
no new messages
no new messages

I tried to start only rtabmapviz on the client, but I didn't succeed. As a starting point I used the launchfile from chapter 5.1 of the manual (Remote_Mapping) and modified it.

<launch>
  <!-- Visualisation (client side) -->
  <group ns="rtabmap">
    <node 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="queue_size" type="int" value="10"/>
      <param name="frame_id" type="string" value="camera_link"/>

      <remap from="rgb/image" to="/kinect2/data_throttled_image"/>
      <remap from="depth/image" to="/kinect2/data_throttled_image_depth"/>
      <remap from="rgb/camera_info" to="/kinect2/data_throttled_camera_info"/>
      
      <param name="rgb/image_transport" type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
    </node>
  </group>
</launch>

Can you please look at my code and tell me if it's all right? I'd like to see at least one camera image to drive safely. Right now i get no warning or error message and all I see is this:

started roslaunch server http://192.168.1.11:38987/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.7
 * /rtabmap/rtabmapviz/depth/image_transport: compressedDepth
 * /rtabmap/rtabmapviz/frame_id: camera_link
 * /rtabmap/rtabmapviz/queue_size: 10
 * /rtabmap/rtabmapviz/rgb/image_transport: compressed
 * /rtabmap/rtabmapviz/subscribe_depth: False

NODES
  /rtabmap/
    rtabmapviz (rtabmap_ros/rtabmapviz)

ROS_MASTER_URI=http://192.168.1.2:11311

core service [/rosout] found
process[rtabmap/rtabmapviz-1]: started with pid [23171]
[ INFO] [1513099755.881005553]: Starting node...
[ INFO] [1513099756.145582915]: rtabmapviz: Using configuration from "/home/bjorn/catkin_ws/src/rtabmap_ros/launch/config/rgbd_gui.ini"
[ INFO] [1513099757.458254732]: Reading parameters from the ROS server...
[ INFO] [1513099759.154948058]: Parameters read = 254
[ INFO] [1513099760.143094973]: /rtabmap/rtabmapviz: queue_size    = 10
[ INFO] [1513099760.143168667]: /rtabmap/rtabmapviz: rgbd_cameras = 1
[ INFO] [1513099760.143215229]: /rtabmap/rtabmapviz: approx_sync   = false
[ INFO] [1513099760.175320931]: 
/rtabmap/rtabmapviz subscribed to:
   /rtabmap/odom
[ INFO] [1513099760.175686968]: rtabmapviz started.

I get the same result when I copy the rtabmapviz part from the rtabmap.launch file. When i set subscribe_depth=true I get the warning:

[ WARN] [1513101861.395385368]: /rtabmap/rtabmapviz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmapviz subscribed to (exact sync):
   /rtabmap/odom,
   /kinect2/data_throttled_image/compressed,
   /kinect2/data_throttled_image_depth/compressedDepth,
   /kinect2/data_throttled_camera_info,
   /rtabmap/odom_info

ROS Master commands are integrated into the .bashrc. Commands used on robot:

roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
roslaunch freenect2_throttle.launch rate:=10
rosrun tf static_transform_publisher 0 0 0 -1.57079667948966 0 -1.5707963267948966 camera_link kinect2_link 100
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rgb_topic:=/kinect2/qhd/image_color_rect depth_registered_topic:=/kinect2/qhd/image_depth_rect camera_info_topic:=/kinect2/qhd/camera_info rtabmapviz:=false

Commands used on client:

roslaunch rtabmap_ros rtabmapviz_remote.launch

Best regards, Wezza

[matlabbe]

If you want to do teleop, maybe using RVIZ would be better as you will have more choices of what you want to render. In RVIZ, you get see any image topic directly too.

For rtabmapviz, on a remote computer, I don't really recommend to use subscribe_depth to avoid high bandwidth topic synchronization. For example, the data_throttled_image_depth is 0.5 Hz while the others are 8 Hz. This is a big problem for synchronization on client side. This can be simplified as this:

<launch>
  <!-- Visualisation (client side) -->
  <group ns="rtabmap">
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
      <param name="frame_id" type="string" value="camera_link"/>
      <remap from="odom" to="odom"/>
    </node>
  </group>
</launch>

Is your robot base frame id really camera_link? Normally, it would be base_link or base_footprint. See ROS REP 105.

In the screenshot you sent, we only see the white odometry line, without the blue graph map. This means that rtabmapviz is not receiving /rtabmap/mapData topic. You can do on client:

$ rostopic hz /rtabmap/mapData

If not receiving at 1 Hz, look for errors/warnings on computer you started rtabmap.

Note that to compare actual frame rate of the topics on the robot and from the outside, compare "rostopic hz" on the robot vs on client computer for the same topic.

If odometry/mapping is done on the robot, you don't need data_throttle node. In RVIZ, you can subscribe to image topic with compression you want directly.

cheers,
Mathieu

[Wezza]

Hello, it's me again! :D :-/

I've listened to you and now use rviz for teleop. I reduced the rate and the resulting bandwidth of the transmitted image topic with the data_throttle node. The "Frame Rate" option under "Global Options" in RVIZ had no influence. And just subscribing the compressed qhd color image requires up to 4 MiB/s.
The output for
rostopic hz /rtabmap/mapData
is average rate ~0.88 (after a few minutes) on robot and client side.

If i start RVIZ on the robot everything works as expected. But on the client i'am not able to get any data for "MapCloud" --> Topic /rtabmap/mapData :-( The rtabmap node on the roboter throws the following error:

[ERROR] [1513179083.761802928]: Client [/rviz] wants topic /rtabmap/mapData to have datatype/md5sum [rtabmap_ros/MapData/a1816275c0864a26aefc5ed10220a99f], but our version has [rtabmap_ros/MapData/ceb3cc5db87e14b6dac5adad9c3f8a98]. Dropping connection.

Maybe the error occurs because on client side rtabmap_ros is build from source and on robot side it is installed via apt-get?

If I click in RVIZ (robot side) on MapCloud --> "Download map" or "Download graph" the following error message opens:
"MapCloudDisplay: Can't call "/rtabmap/get_map_data" service. Tip: if rtabmap node is not in rtabmap namespace, you can remap the service to "get_map_data" in the launch file like: ."
MapGraph is also not working. RVIZ looks like this:



This is my launchfile for RVIZ, works as expected on the robot but not on the client:

<launch>
  <arg name="approx_sync"             default="true"/>         <!-- if timestamps of the input topics are not synchronized -->
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />

  <!-- Visualization RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/camera_info"   to="/kinect2/data_throttled_camera_info"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>
</launch>

When I add a pointCloud2 and subscribe to rtabmap/cloud_map I get the following result.



At the moment, camera_link is my base_link because I removed the camera from the robot for easier testing.

Best regards, Wezza

[matlabbe]

Hi Wezza,

You may not want to use "standalone rtabmap_ros/point_cloud_xyzrgb" on client side, as it should connect to rgb/depth/camera_info topics, thus generating bandwidth. Unless you need to see live point cloud from the kinect, you may drop this node. Tips: if you have on the client side multiple nodes connecting to same topics, use relays to avoid duplicating connection between robot and client (e.g. retransmitting only once the RGB image on network instead of multiple times it has more than one nodes subscribing to it on client).

The /rtabmap/map_cloud topic is a general Pointcloud2 topic, it is the 3D map created from rtabmap. It is however less efficient than /rtabmap/mapData for visualization and resolution is limited to grid cell used by rtabmap (default 5 cm). Your error about mapData is indeed because the message defintion is different between the robot and the client, caused by different rtabmap versions installed between the computers.

cheers,
Mathieu

[Wezza]

Thanks a lot! You safed my life

I build rtabmap from source on the robot, removed "standalone rtabmap_ros/point_cloud_xyzrgb" from the remote_rviz launchfile and now everything works!

Just one more question ^^ When I drive around and reach a point where I already was, RVIZ stops updating my map. loop closure right? How can i tell RVIZ to move on with mapping?

Best regards,
Wezza

[matlabbe]

Hi Wezza,

Which topic are you visualizing? mapData, grid_map, cloud_map? RVIZ just shows what it is receiving, if it doesn't refresh anymore, maybe it doesn't receive the topics (wifi lag for example). With or without loop closures, the map should be refreshed at 1 Hz. When not moving, the map may not be updated though.

cheers,
Mathieu

[Wezza]

Hi Mathieu!

I had visualized rtabmap/mapData in rviz. I don't know what the error was, but I didn't feel like looking for him anymore ^^ I also noticed another error. As soon as I wanted to take a closer look and move/rotate the 3D map in rtabmapviz (on the robot, see if mapping also stops) the graphical user interface crashed. When I interchanged client and robot in tests everything worked without any problems. I think some dependencies in the background were wrong. So I decided to format the robots laptop and reinstall everything. Now it works as expected, on both sides.

So far, thank you for your help and tips!

I warn you in advance, on Monday I will start with multi sessions ;-)

Best regards,
Wezza

[Wezza]

Hi and happy new year Mathieu!

I have a question regarding the visualization of the robot in RVIZ. I've build a simple URDF model from my robot and i want this model to move in my map in RVIZ (like the kinect v2 coordinate systems) as soon as my robot moves in real world and builds the map. My fixed frame in RVIZ is "map". But how can i transform from map to base_link? Is this even the right approach? Should I add a static_transform_publisher to my rtabmap launchfile? I don't have any idea at the moment how I could solve my problem.

If you need more information, like URDF File, rtabmap launch File, rviz launch file... please let me know.

Best regards

Wezza

[matlabbe]

Hi,

you should make the kinect frame the child of your robot frame, then use the robot base frame instead of the kinect for rtabmap's frame_id parameter. Not sure what is your current setup, but if visual odometry is still used, you should have a TF tree similar to /map -> /odom ->/base_link -> /kinect2_link... where /map -> /odom transform is published by rtabmap, /odom->/base_link would be published by rgbd_odometry and /base_link -> /kinect2_link by your URDF. Well, /base_link is the standard frame name for the base frame of the robot, it could be also /base_footprint on some other robots. In this case, rtabmap's frame_id should be "base_link" or "base_footprint" (should be the frame of the root of the URDF).

cheers,
Mathieu

[Wezza]

Hi Mathieu

i changed everything as you said and it worked just fine.

Yesterday I had to update (apt-get update and apt-get upgrade) the remote system after a Ubuntu boot loop error. Now, when i start rviz on my remote computer, the following error occurred:

[ERROR] [1516105333.833028903]: PluginlibFactory: The plugin for class 'rtabmap_ros/MapCloud' failed to load.  Error: Failed to load library /home/bjorn/catkin_ws/devel/lib//librtabmap_ros.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libopencv_core3.so.3.2: Cannot open the shared object file: file or directory not found)

I think the error occurs because there is an additional slash in the path (catkin_ws/devel/lib//librtabmap_ros.so), so the path is incorrect. I don't know what caused the error, because I haven't changed anything. Could it have been caused by the ROS update from yesterday? Do you know how to fix it?

Best regards, Wezza

---------------------------------------------
Edit:

Oh god, sorry.
I just rerun catkin_make and it fixed the problem.

Best regards, Wezza