Mapping performance and hallway problems with Kinect 2

Posted by Wezza on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Mapping-performance-and-hallway-problems-with-Kinect-2-tp3956.html

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>