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-tp5517p5545.html

Hi,

You are not using visual or icp odometry in your launch file, you are using odometry from the robot (wheel odometry).

Documentation about RTAB-Map's parameters can be printed with:

$ rosrun rtabmap_ros rtabmap --params | grep Icp/MaxCorrespondenceDistance
Param: Icp/MaxCorrespondenceDistance = "0.1"               [Max distance for point correspondences.]

Icp/MaxCorrespondenceDistance is used for refining loop closure detection and do proximity detection. When doing scan matching, only points under this distance are considered for ICP.

To set in your launch file, add the following under rtabmap node:

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

Here is an example of poor synchronization between rgb and depth:


Yes, approx_sync should be false. The problem is that in your launch file, we should pre-sync kinect image because you are also subscribing to a scan topic (coming from a different sensor). This can be done like in figure 6 of this paper, using rgbd_sync node. In your launch file, this would look like this:

<?xml version="1.0" encoding="UTF-8"?>

<launch>
  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="rgb_topic"         default="/kinect2/qhd/image_color_rect"/>
  <arg name="depth_topic"       default="/kinect2/qhd/image_depth_rect"/>
  <arg name="camera_info_topic" default="/kinect2/qhd/camera_info"/> 

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

     <!-- pre-sync kinect images before rtabmap node -->
     <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync">
      <remap from="rgb/image"         to="$(arg rgb_topic)"/>
      <remap from="depth/image"       to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
      <remap from="rgbd_image"        to="rgbd_image"/> <!-- output -->
      <param name="approx_sync"       value="false"/>   <!-- exact sync for kinect2 topics -->
    </node>

    <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"/>

      <!-- 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>

cheers,
Mathieu