Re: Troubleshooting mapping with kinect v2, rplidar and jetson tx2

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Troubleshooting-mapping-with-kinect-v2-rplidar-and-jetson-tx2-tp5517p5562.html

Hi,

I wasn't sure from where the throttled topics were coming from. I re-read your first post, and I see that you are running kinect2_bridge and lidar on the jetson, and rtabmap on remote computer. In that kind of setup, rgbd_sync should be launched on the jetson. With kinect2_bridge, I suggest to launch it by setting the fps_limit parameter instead of throttling the images. Otherwise throttling could be combined with rgbd_sync on the kinect2_bridge nodelet manager for efficiency on the jetson:

<launch>
  <arg name="rate"     default="10"/>
  <arg name="decimation"     default="1"/>
  <!-- Use same nodelet used by kinect2_bridge -->
  <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="false"/> <!-- exact sync for kinect2 topics -->

      <remap from="rgb/image_in"       to="/kinect2/qhd/image_color_rect"/>
      <remap from="depth/image_in"     to="/kinect2/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info_in" to="/kinect2/qhd/camera_info"/>

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

  <!-- pre-sync kinect images before sending them over network -->
  <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync kinect2">
      <remap from="rgb/image"         to="/kinect2/qhd/data_throttled_image"/>
      <remap from="depth/image"       to="/kinect2/qhd/data_throttled_image_depth"/>
      <remap from="rgb/camera_info"   to="/kinect2/qhd/data_throttled_camera_info"/>
      <remap from="rgbd_image"        to="/rtabmap/rgbd_image"/> <!-- output -->
      <param name="approx_sync"       value="false"/>   <!-- exact sync for kinect2 topics -->
  </node>
</launch>

Then as rtabmap is on remote computer, we should subscribe to compressed rgbd_image topic for bandwidth efficiency (if it is connected on LAN, you may subscribe to raw topic to save some compression time on the jetson):

<launch>
  <arg name="database_path"     default="rtabmap.db"/>

  <!-- Mapping Node -->
  <group ns="rtabmap">

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">

      <!-- Basic RTAB-Map Parameters -->
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="robot_footprint"/> <!--"robot_footprint"/-->
      <param name="odom_frame_id"       type="string" value="odom"/>
      <param name="odom_tf_angular_variance" type="double" value="0.005"/>
      <param name="odom_tf_linear_variance"  type="double" value="0.005"/>
      <param name="subscribe_depth"     type="bool"   value="false"/>
      <param name="subscribe_rgbd"      type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="true"/>
      <param name="subscribe_odom"      type="bool"   value="true"/>
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
      <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
      <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      <param name="queue_size" type="string" value="20"/>

      <!-- RTAB-Map Inputs -->
      <remap from="scan"       to="/scan"/>
      <remap from="rgbd_image" to="rgbd_image/compressed"/>

      <!-- RTAB-Map Output -->
      <remap from="grid_map" to="/map"/>

      <!-- Rate (Hz) at which new nodes are added to map -->
      <param name="Rtabmap/DetectionRate" type="string" value="1"/> 

      <!-- 2D SLAM -->
      <param name="Reg/Force3DoF" type="string" value="true"/>      

      <!-- Loop Closure Detection -->
      <!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE-->
      <param name="Kp/DetectorStrategy" type="string" value="3"/> 

      <!-- Maximum visual words per image (bag-of-words) -->
      <param name="Kp/MaxFeatures" type="string" value="500"/>  

      <!-- Used to extract more or less SURF features -->
      <param name="SURF/HessianThreshold" type="string" value="150"/>

      <!-- Loop Closure Constraint -->
      <!-- 0=Visual, 1=ICP (1 requires scan)-->
      <param name="Reg/Strategy" type="string" value="1"/> 

      <!-- Minimum visual inliers to accept loop closure -->
      <param name="Vis/MinInliers" type="string" value="12"/> <!--10-->

      <!-- Set to false to avoid saving data when robot is not moving -->
      <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>

      <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

    </node> 
  </group>

  <!--launch rviz-->
  <node name="rviz" pkg="rviz" type="rviz" respawn="false"/>
</launch>

I looked at the database, the 2D map looks good, only thing remaining is the poor rgb/depth synchronization. With rgbd_sync running on jetson like above, I think this will help.

cheers,
Mathieu