test_velodyne.launch with data from kitti.

[Mikor]

Hi Mathieu,

I am trying to make the test_velodyne launchfile to take as input the data from the kitti dataset. The topics contained in a bag comming from KITTI are as follows:

topics:      /kitti/camera_color_left/camera_info    78 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_color_left/image_raw      78 msgs    : sensor_msgs/Image         
             /kitti/camera_color_right/camera_info   78 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_color_right/image_raw     78 msgs    : sensor_msgs/Image         
             /kitti/camera_gray_left/camera_info     78 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_gray_left/image_raw       78 msgs    : sensor_msgs/Image         
             /kitti/camera_gray_right/camera_info    78 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_gray_right/image_raw      78 msgs    : sensor_msgs/Image         
             /kitti/oxts/gps/fix                     78 msgs    : sensor_msgs/NavSatFix     
             /kitti/oxts/gps/vel                     78 msgs    : geometry_msgs/TwistStamped
             /kitti/oxts/imu                         78 msgs    : sensor_msgs/Imu           
             /kitti/velo/pointcloud                  78 msgs    : sensor_msgs/PointCloud2   
             /tf                                     78 msgs    : tf2_msgs/TFMessage        
             /tf_static                              78 msgs    : tf2_msgs/TFMessage

I took the test_velodyne and edit it as follows:

<launch>
  
    <!-- 
    Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
    Prerequisities: rtabmap should be built with libpointmatcher
    Example:
     $ roslaunch rtabmap_ros test_velodyne.launch
     $ rosrun rviz rviz -f map
     $ Show TF and /rtabmap/cloud_map topics
    -->

    <arg name="rtabmapviz"    default="true"/>
    <arg name="use_imu"       default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
    <arg name="imu_topic"     default="/kitti/oxts/imu"/>
    <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
    <arg name="use_sim_time"  default="false"/>
    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
    
    <arg name="frame_id" default="base_link"/>
    <arg name="scan_cloud_topic"        default="/kitti/velo/pointcloud"/>
    
    <!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
    <node if="$(arg use_imu)" pkg="rtabmap_ros" type="imu_to_tf" name="imu_to_tf">
      <remap from="imu/data" to="$(arg imu_topic)"/>
      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
      <param name="base_frame_id" value="$(arg frame_id)"/>
    </node> 

    <group ns="rtabmap">
      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
        <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>

        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/>
        <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
        <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
     
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="10"/>
        <param name="Icp/VoxelSize"           type="string" value="0.2"/>
        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
        <param name="Icp/Epsilon"             type="string" value="0.001"/>
        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
        <param name="Icp/PM"                  type="string" value="true"/> 
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  

        <!-- Odom parameters -->       
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
        <param name="Odom/Strategy"              type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>      
      </node>

      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="false"/>
        
        <remap from="scan_cloud" to="assembled_cloud"/>
        <remap from="imu" to="$(arg imu_topic)"/>
     
        <!-- RTAB-Map's parameters -->
        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
        <param name="Mem/STMSize"                    type="string" value="30"/>
        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->
        
        <param name="Reg/Strategy"                   type="string" value="1"/> 
        <param name="Grid/CellSize"                  type="string" value="0.1"/>
        <param name="Grid/RangeMax"                  type="string" value="20"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>

        <!-- ICP parameters -->
        <param name="Icp/VoxelSize"                  type="string" value="0.2"/>
        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
        <param name="Icp/PointToPlane"               type="string" value="true"/>
        <param name="Icp/Iterations"                 type="string" value="10"/>
        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
        <param name="Icp/MaxTranslation"             type="string" value="3"/>
        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
        <param name="Icp/PM"                         type="string" value="true"/> 
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="0.4"/>
      </node>

      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
        <param name="frame_id" type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="subscribe_odom_info" type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync" type="bool" value="false"/>
        <remap from="scan_cloud" to="/velodyne_points"/>
      </node>

      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
        <remap from="cloud"           to="/velodyne_points"/>
        <remap from="odom"            to="odom"/>
        <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
        <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
        <param name="fixed_frame_id"  type="string" value="" />
      </node>
  </group>


</launch>

And here are my questions:

[0] What are the necessary changes in order to do that, because as I did it seems that it does not take the data in consideration.

I made the necessary changes so that the nodes now take in consideration the appropriate topics but my results are extermely messy.

[1] When we "declare" a node what is the use of the type part of it ? I realise that it matches with the scripts in the source code, so I guess it corresponds with the script of the package that is about to run in that node.

[2]  Why are the ICP parameters declared twice one time in the icp_odometry node, which makes sense to me and another in the rtabmap node?

[3] What is the use of the point_cloud_assembler nodelet?

[4] Also, how the imu_to_tf is used? I see that it creates a tf between fixed frame id and base frame id I do not understand why and how it is used.

[matlabbe]

Hi,

[0] What are the necessary changes in order to do that, because as I did it seems that it does not take the data in consideration.

What warning messages do you get?

[1] The "node" tag contains the package ("pkg") it is coming from, the actual type of the node you want to start ("type") and a unique name ("name"). Hving multiple rtabmap nodes, you would have to use different names.

[2] Both Icp parameters are used in both nodes and they can differ from each others. This example is more compact: http://official-rtab-map-forum.206.s1.nabble.com/Is-this-the-way-to-use-lidar-IMU-efficiently-tp7496p7515.html , where we declare one time Icp parameters and they are fed to both nodes.

[3] In this example, we wanted to assemble ALL scans from velodyne. Otherwise, only one scan at each rtabmap update will be kept.

[4] I did some changes recently to use IMU topic directly in icp_odometry to avoid creating that fake TF. This approach is still valid, it was used to give a rotation guess for ICP by using TF.

cheers,
Mathieu

[Mikor]

[0] I fixed it, I did't subscribe rtabmap to the suitable topic of the pointclouds.

[1] Okay, now I get it.

[2] I see that ICP is used in both nodes (icp_odometry and rtabmap) but I was under the impression that ICP is done only for odometry so it can calculate the way that the "robot" moved. But I can not understand why ICP is used in the rtabmap node, is it used for the creation of the pointcloud ?

[3] Okay, I see, so this nodelet allows every pointclouds to be included in the final pointcloud otherwise only the last one would be. (Am I right ?)

[4] I see, sometimes when I use it behaves very strictly and discards the new nodes.

[matlabbe]

[3] On loop closure detection and proximity detection, a transformation has to be computed, so ICP can be used there.

[4] yes!