MarvinStuede / cmr_lidarloop

Method to add Lidar based loop detections to Graph based SLAM
BSD 3-Clause "New" or "Revised" License
17 stars 4 forks source link

cmr_lidarloop not working (Grid map is empty!) #8

Open sj1108 opened 2 years ago

sj1108 commented 2 years ago

Hello,

I want to build a map using rtamap_ros+cmr_lidarloop package. I am trying to build a map with only lidar+imu sensor (no camera) attached to a vehicle. As far as I know, rtabmap_ros doesn't provide loop-closing function with only lidar+imu sensor configuration. But your work can provide this function, is that correct?

I have built the lidarloop package and followed the instruction. But I got stuck with some warning messages that I cannot understand. Beforehand I checked the relevant questions #4 and #5. I think I did all the things that I should set. Unfortunately I failed to make it work. Although the launch file is run and I checked that cmr_lidarloop/scan_descriptor topic is coming out, but map is not created.

Below is the launch file that I made, referencing many other launch files.

<?xml version="1.0"?>
<launch>
  <arg name="mapping" default="true" doc="If true, overwrite some params for mapping"/>
  <arg name="cfg_file" default="$(find cmr_lidarloop)/cfg/cmr_lidarloop_params.yaml"/>

  <node pkg="tf" type="static_transform_publisher" name="vehicle_to_velodyne"  
    args="-1.1 0 1.7 1.570796 0 0 vehicle_frame VELODYNE_VLP_32C_TOP_REAR_CENTER 100"/>
  <node pkg="tf" type="static_transform_publisher" name="vehicle_to_imu"  
    args="-1.3 0 0.8 -1.570796 0 0 vehicle_frame imu 100" />

  <group ns="cmr_lidarloop">
  <env name="ROSCONSOLE_CONFIG_FILE" value="$(find cmr_lidarloop)/cfg/rosconsole.config"/>
    <!-- Loads main params from yaml -->
    <rosparam file="$(arg cfg_file)" command="load" />

    <group if="$(arg mapping)">
        <!-- Overwrite param to wait a duration until next detection -->
        <param name="throttle_dur" type="double" value="0.0"/>
    </group>

    <!-- Start main node -->
    <node name="lidar_loopdetection" pkg="cmr_lidarloop" type="lidar_loopdetection" output="screen" respawn="true">
    </node> 

    <!-- Start registration server -->
     <node name="lidar_registration_server" pkg="cmr_lidarloop" type="lidar_registration_server" output="screen" respawn="true">
     </node>

    <!-- Start loop detector server -->
    <node name="lidar_loopdetector_server" pkg="cmr_lidarloop" type="lidar_loopdetector_server.py" output="screen" respawn="true">
     </node>
  </group>

  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="depth"            value="false"/>
    <arg name="stereo"           value="false"/>
    <arg name="subscribe_rgb"    value="false"/>
    <arg name="rtabmapviz"       value="false"/>
    <arg name="localization"     value="false"/>
    <arg name="use_sim_time"     value="true"/>
    <arg name="frame_id"         value="vehicle_frame"/>
    <arg name="database_path"    value="~/.ros/rtabmap.db"/>
    <arg name="approx_sync"      value="false"/>
    <arg name="queue_size"       value="10"/>

    <arg name="subscribe_scan_cloud"       value="false"/>
    <arg name="scan_cloud_topic"           value="/velodyne_points"/>
    <arg name="subscribe_scan_descriptor"  value="true"/>
    <arg name="scan_descriptor_topic"      value="/cmr_lidarloop/scan_descriptor"/>
    <arg name="scan_cloud_max_points"      value="60000"/>
    <arg name="scan_cloud_filtered"        value="true"/>

    <arg name="visual_odometry"    value="false"/>
    <arg name="icp_odometry"       value="true"/>
    <arg name="odom_topic"         value="scanmatch_odom" />
    <arg name="imu_topic"          value="/an_device/Imu" />
    <arg name="wait_imu_to_init"   value="true" />
    <arg name="odom_expected_rate" value="15" />

    <arg name="scan_cloud_assembling"             value="true"/>
    <arg name="scan_cloud_assembling_time"        value="0"/>
    <arg name="scan_cloud_assembling_max_clouds"  value="10"/>
    <arg name="scan_cloud_assembling_voxel_size"  value="0.3"/>

    <arg name="gps_topic"        value="/an_device/NavSatFix"/>

    <arg name="odom_args"       value="--Icp/PointToPlane true
      --Icp/VoxelSize 0.3
      --Icp/Iterations 10
      --Icp/VoxelSize 0.3
      --Icp/DownSamplingStep 1
      --Icp/Epsilon 0.001
      --Icp/PointToPlaneK 20
      --Icp/PointToPlaneRadius 0
      --Icp/MaxTranslation 2
      --Icp/MaxCorrespondenceDistance 1
      --Icp/PM true
      --ICP/PMOutlierRatio 0.1
      --Icp/CorrespondenceRatio 0.01
      --Icp/PointToPlaneGroundNormalsUp 0.8
      --Odom/ScanKeyFrameThr 0.8
      --Odom/Strategy 0
      --OdomF2M/ScanSubtractRadius 0.3
      --OdomF2M/ScanMaxSize 15000
      --OdomLOAM/Sensor 1
      --OdomLOAM/Resolution 0.3
      --OdomLOAM/ScanPeriod 0.1
    "/>

    <arg name="rtabmap_args"    value="--delete_db_on_start
      --Rtabmap/DetectionRate 1
      --Mem/NotLinkedNodesKept false
      --Mem/STMSize 30
      --Reg/Strategy 1
      --Grid/CellSize 0.3
      --Grid/RangeMax 20
      --Grid/ClusterRadius 1
      --Grid/GroundIsObstacle false
      --Optimizer/GravitySigma 0.3
      "/>
  </include>

  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/rviz/my_test.rviz" output="screen"/>

</launch>

This is the yaml file that I set.

#* @file   cmr_lidarloop_params.yaml
#* @author Tim-Lukas Habich
#* @date   05/2020
#*
#* @brief  Parameters of cmr_lidarloop package
#*
#

#Loop detection parameters
#Use standalone environment /cmr_lidarloop/detector to train and test loop detectors
loop_probability_min:   0.524                     #Minimum value for the loop probability, to accept a loop
R_min:                  7.5                       #Minimum radius in which loops are searched -> added to this is the current uncertainty of the position (computed with odometry)
r_max:                  50.0                      #Maximum range for feature computation
n_verify:               3                         #Verify loop -> detect at least another loop between current id and [loop_id-n_verify,loop_id+n_verify]
                                                  #n_verify<1 -> disabled
n_max_nodes:            200                       #Maximum number of nodes used for loop search -> if more nodes are available, a random subset of size n_max_nodes is used
alpha_thres:            0.5                       #Multi session operation: alpha=nodes_WM_local_map/nodes_WM_all
                                                  #alpha<alpha_thres -> Localization in map from different session not yet done -> Search for loops throughout WM
n_ms_verify:            3                         #Verification for multi session operation: n_ms_verify consecutive loop candidates must lie within radius R_ms_verify
R_ms_verify:            5.0                       #Verification for multi session operation: n_ms_verify consecutive loop candidates must lie within radius R_ms_verify
n_ms_start:             3                         #Multi session operation: for the first added n_ms_start loop pairs search for loops throughout WM instead within R_search
beta:                   0.25                      #Radius in which loops are searched R_search=R_min+beta*R_odom

#Scan registration parameters
#Use standalone environment /cmr_lidarloop/src/Registration_Test to test registration with desired scans
sky_direction:          3                         #Axis pointing to the sky to identify z-axis in the current coordinate system
                                                  #1->x pointing to the sky, -1->-x pointing to the sky, 
                                                  #2->y pointing to the sky, -2->-y pointing to the sky, 
                                                  #3->z pointing to the sky, -3->-z pointing to the sky,
                                                  #0->z trimming disabled
z_limit:                1.0                       #Z-coordinate at which the point clouds are trimmed (to avoid random points on the ground)
i_limit:                0                         #Intensity filter: Delete points in point cloud with intensity<i_limit
r_limit:                30                        #Range filter: Delete points in point cloud with range>r_limit
leafsize:               0.2                       #Leaf size for voxel grid filter
n_max_points:           10000                     #Random downsampling after all filter steps, if number of points in scan is bigger than n_max_points
n_min_points:           7000                      #Both filtered clouds should have more points than n_min_points. Otherwise loop pair is rejected, because registration is too challenging
min_inliers:            1000                      #Minimum number of inliers to accept a transformation (after outlier rejection)
t_max:                  3.0                       #Maximum translational offset. Only if the translational offset calculated from the LiDAR registration is smaller, 
                                                  #the loop is accepted and the link is sent to RTAB-Map
path_clouds:            '~/.ros/lidarloop_clouds' #Path where the point clouds of every registered loop pair are to be stored. If 'false' no clouds are saved
throttle_dur: 45.0                                #When localized, wait at least this duration until next iteration. This avoids heavy CPU load.
                                                  #Will be overriden to 0.0 for mapping
#Topics
scan_topic_name:        '/velodyne_points'        #Name of the topic on which the laser scanner is publishing
odom_topic_name:        '/rtabmap/scanmatch_odom' #Name of the topic on which the odometry data is published

The error message:

[ INFO] [1665057776.808785862, 1664869704.833422425]: Odom: ratio=0.849843, std dev=0.043421m|0.013731rad, update time=0.047602s
[ WARN] [1665057776.831045624, 1664869704.863684216]: Grid map is empty! (local maps=1)
[ INFO] [1665057776.831075328, 1664869704.863684216]: rtabmap (2): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0213s, Maps update=0.0004s pub=0.0000s (local map=1, WM=1)
[ INFO] [1665057776.909952484, 1664869704.944285858]: Odom: ratio=0.843105, std dev=0.049051m|0.015511rad, update time=0.049073s
[ INFO] [1665057777.009828884, 1664869705.035033121]: Odom: ratio=0.840778, std dev=0.050604m|0.016002rad, update time=0.049617s
[ INFO] [1665057777.115733680, 1664869705.145874866]: Odom: ratio=0.832422, std dev=0.057987m|0.018337rad, update time=0.054535s
[ INFO] [1665057777.221204684, 1664869705.246634190]: Odom: ratio=0.834998, std dev=0.057364m|0.018140rad, update time=0.057974s
[ INFO] [1665057777.315961686, 1664869705.347440112]: Odom: ratio=0.825546, std dev=0.054405m|0.017204rad, update time=0.051188s
[ INFO] [1665057777.413664903, 1664869705.438128031]: Odom: ratio=0.824490, std dev=0.050509m|0.015972rad, update time=0.053089s
[ INFO] [1665057777.518749425, 1664869705.548898498]: Odom: ratio=0.815316, std dev=0.050365m|0.015927rad, update time=0.052566s
[ INFO] [1665057777.608561136, 1664869705.639613649]: Odom: ratio=0.804495, std dev=0.051276m|0.016215rad, update time=0.047035s
[ INFO] [1665057777.713690737, 1664869705.740345285]: Odom: ratio=0.809586, std dev=0.053566m|0.016939rad, update time=0.052066s
[ INFO] [1665057777.844088779, 1664869705.871372068]: Odom: ratio=0.799286, std dev=0.056706m|0.017932rad, update time=0.079725s
[ WARN] (2022-10-06 21:02:57.869) SensorData.cpp:681::uncompressDataConst() Requested raw image data, but the sensor data (3) doesn't have image.
[ WARN] (2022-10-06 21:02:57.869) SensorData.cpp:696::uncompressDataConst() Requested depth/right image data, but the sensor data (3) doesn't have depth/right image.
[ INFO] [1665057777.869642419, 1664869705.901604867]: rtabmap (3): Rate=1.00s, Limit=0.000s, Conversion=0.0009s, RTAB-Map=0.0235s, Maps update=0.0012s pub=0.0001s (local map=2, WM=2)
[ WARN] (2022-10-06 21:02:57.872) SensorData.cpp:681::uncompressDataConst() Requested raw image data, but the sensor data (3) doesn't have image.
[ INFO] [1665057777.890792740, 1664869705.921749012]: Creating 1 swatches
[ INFO] [1665057777.930320997, 1664869705.962056620]: Odom: ratio=0.877882, std dev=0.042486m|0.013435rad, update time=0.067368s

This is the bag file that I used.

Can you help me? Any suggestion or advice will be really grateful. Many thanks.