Localization after mapping session using lidar only

[Bracky]

Hello Matthieu, Hello everyone,
So I am trying to localize in 2D my robot. The robot's task are 1) leave station 2) do stuff 3) return to station. Which is why when leave, I am trying to create a map in which I can localize in for returning to station. However, I only have a lidar.
Creating the map works but localizing in it does not.
 
One (or more) of my parameters might be incorrect in my launch file. Any ideas ?

<launch>       
<!-- Choose visualization -->
<arg name="rviz"    default="true" />

<arg name="use_imu" default="true" />
<arg name="imu_topic" default="truc/truc" />
<arg name="localization" value="true"/>

<arg     if="$(arg localization)" name="rtabmap_args"  default="localization:=true"/>
<arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
    
<!-- Point to Plane ICP? -->
<arg name="p2n" default="false" />
    
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
             <remap from="scan_cloud"      to="/lidar/points"/>
             <remap from="odom_info"      to="/rtabmap/odom_info"/>
            <param name="subscribe_scan"        type="bool"     value="false"/>
            <param name="subscribe_scan_cloud"  type="bool"     value="true"/>
            <param name="odom_frame_id"         type="string"   value="odom"/>
            <param name="frame_id"              type="string"   value="base_link"/>
            <param name="deskewing"             type="string"   value="true"/>
            <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/>
            <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="base_link"/>
            <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
            <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
            <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
            <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
            <param unless="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="false"/>                       
            <param name="Icp/MaxCorrespondenceDistance"     type="double"      value="10"/>
            <param name="Icp/Iterations"                    type="int"      value="100"/>
            <param name="Icp/Strategy"                      type="int"      value="1"/>
            <param name="Icp/VoxelSize"     type="string" value="0.05"/>
            <param name="Icp/RangeMax"      type="string" value="0"/>
            <param name="Icp/Epsilon"       type="string" value="0.001"/>
            <param name="Icp/MaxTranslation" type="string" value="3"/>
            <param if="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="true"/>
            <param if="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="5"/>
            <param if="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
            <param unless="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
            <param unless="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
            <param unless="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0"/>
            <param name="Icp/MaxCorrespondenceDistance" type="string" value="10"/>
            <param name="Odom/Strategy"        type="string" value="0"/>
            <param name="Odom/FilteringStrategy" type="int"  value="1"/>
            <param name="Odom/GuessMotion"     type="string" value="true"/><!-- can't guess transform from null transform-->
            <param name="Odom/ResetCountdown"  type="string" value="0"/>
            <param name="Odom/ScanKeyFrameThr"  type="string" value="0.9"/>
        </node>

        <node name="rtabmap" pkg="rtabmap_ros"  type="rtabmap"  output="screen" args="$(arg rtabmap_args)">
            <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
            <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
            <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
            <param unless="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="false"/>
            <!--topics related-->
            <param name="queue_size"            type="int"      value="10"/>
            <param if="$(arg localization)"     name="publish_tf"            type="bool" value="false"/>
            <param name="tf_delay"              type="double"   value="0.05"/>
            <param name="tf_prefix"             type="string"   value=""/>
            <param name="wait_for_transform"    type="bool"     value="true"/>
            <!-- devices -->
            <param name="subscribe_depth"       type="bool"     value="false"/>
            <param name="subscribe_rgb"         type="bool"     value="false"/>
            <param name="subscribe_scan"        type="bool"     value="false"/>
            <param name="subscribe_scan_cloud"  type="bool"     value="true"/>            
            <remap from="/gps/fix"          to="/rr100/navsat/fix"/>
            <remap from="scan_cloud"        to="/lidar/points"/>
            <!-- frames -->
            <param name="frame_id"              type="string"   value="base_footprint"/>
            <param name="map_frame_id"          type="string"   value="map"/>
            <param name="odom_frame_id"         type="string"   value="odom"/>
            <param name="publish_tf"            type="bool"     value="true"/>
            <param if=    "$(arg localization)"    name="database_path"         type="string"   value="~/.ros/rtabmap_clean.db"/>
            <param unless="$(arg localization)"    name="database_path"         type="string"   value="~/.ros/rtabmap.db"/>
            <!--mapping-->	
            <param name="map_cleanup"               type="bool"     value="false"/>
            <param name="map_always_update"         type="bool"     value="true"/>
            <param name="map_empty_ray_tracing"     type="bool"     value="true"/>            
            <param name="Rtabmap/DetectionRate"     type="double"   value="5"/>
            <param name="Rtabmap/LoopGPS"           type="bool"     value="false"/>
            <param name="Odom/Holonomic"            type="bool"     value="false"/>
            <param name="Odom/AlignWithGround"      type="bool"     value="true"/>
            <param name="Reg/Strategy"              type="int"      value="1"/><!--0=Vis, 1=Icp, 2=VisIcp-->
            <param name="Reg/Force3DoF"             type="bool"     value="true"/>
            <param name="Icp/Strategy"              type="int"      value="1"/>
            <param name="Icp/CorrespondenceRatio"   type="string"   value="0.2"/>
            <param name="Icp/VoxelSize"             type="string"   value="0.05"/>
            <param name="Icp/RangeMax"              type="string"   value="0"/>
            <param name="Grid/RangeMax"             type="string"   value="0"/>
            <param name="Grid/Sensor"               type="int"      value="0"/>
        </node>
    </group>
    <!-- Visualisation RVIZ -->
    <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
</launch>

To get this result I 1) launch my gazebo world, 2) move the robot and the node above in SLAM mode 3) stop the node above 4) move my robot 5) start the node above in localization mode.
I believe that loop closure is required for localization and it seems that there is no "bag of words" as I am using ICP, so a simple icp_odometry allowing high jump should do the job ?

Thanks in advance for your advices and your help ;)

[matlabbe]

RTAB-Map doesn't do global lidar-only localization. Its global localization approach is based on vision. Without a camera it can only localize locally with the lidar, but you will need to give a first guess with RVIZ 2D pose tool (set topic to "/rtabmap/initialpose") to initialize the position of the robot.

[Bracky]

Hello Mathieu,
First of all merry Christmas and thank you for your feedback. Ok so I can switch to localization mode and provide initial guess of robot's pose using services to try to localize. But I do not get why your are pssimistic about localization. If I have a point cloud that I can match with a map I should be able to localize in this map using the ICP by obtaining the matched transform, right ? as the robot may work at night visual feature matching is not possible :/ Also it could be a great feature in RtabMap !
Anyway, have a nice weekend !

[matlabbe]

Thanks, Happy Holidays!

ICP would converge to a local minimum if the localization guess is not good. For global lidar-based localization, descriptors in the point clouds (e.g., https://pointclouds.org/documentation/group__features.html, https://github.com/MarvinStuede/cmr_lidarloop, https://github.com/ethz-asl/segmap) would have to be extracted and compared to whole map to find similar geometries. This could give a first guess, then ICP would refine the localization. This is indeed possible to do, but not implemented officially in rtabmap repo.

cheers,
Mathieu

[Bracky]

Hello again Matthieu,
I followed your advices and used cmr_lidarloop which is great.
However I did some parameter tweaking recently and now I cannot :
- remove the voxel size warning messages : should I set ICP/Voxel_size or scan_voxel size ? anyway tried both but the message "size too small" remained even with size = 0.5 :c --> ([ WARN] (2023-01-13 16:48:58.342) util3d_filtering.cpp:539::voxelizeImpl() Leaf size is too small for the input dataset. Integer indices would overflow. We will split space to be able to voxelize (lvl=0 cloud=131072 min=[-70.677414 -32.900803 -1.001473] max=[107.780090 93.441452 19.073912] voxel=0.050000).)

- merge WM clouds, neither using rtabmap with use_scan_clouds or use_scan_descriptor from cmr_lidarloop. I do not know where to look into : either robot_localization or rtabmap.

global ekf :
#global so tf map->odom
odom_frame: odom
base_link_frame: base_footprint
world_frame: map
map_frame : map
predict_to_current_time : true
transform_time_offset: 0.000
print_diagnostics: true
reset_on_time_jump : true

dynamic_process_noise_covariance : true
two_d_mode: true
permit_corrected_publication : false
smooth_lagged_data : true
history_length : 1.2
frequency: 30
initial_state: [0.0,  0.0,  0.0,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0]

imu0: /imu/data/madgwicked
imu0_config: [false, false, false,
              true, true, true,
              false, false, false,
              true, true, true,
              true, true, true]
imu0_differential: false
imu0_relative: false
imu0_queue_size: 7
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true



odom0 : /odometry/navsat_gps
odom0_config : [true,  true,  false,
                false, false, false,
                false, false, false,
                false, false, false,
                false, false, false]
odom0_queue_size: 10
odom0_nodelay: false
odom0_differential: false
odom0_relative: false

odom1: /rr100_steering_controller/odom
odom1_config: [false, false, false,
               false, false, false,
               true, true, false,
               false, false, true,
               false, false, false]
odom1_differential: false
odom1_queue_size: 10

odom2: /rtabmap/odom
odom2_config: [true, true, false,
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]
odom2_differential: false
odom2_relative: false
odom2_queue_size: 10

local ekf
#local so tf odom->base_link
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom
map_frame : map
predict_to_current_time : true
transform_time_offset: 0.000
print_diagnostics: true
reset_on_time_jump : true

dynamic_process_noise_covariance : true
two_d_mode: true
permit_corrected_publication : false
smooth_lagged_data : true
history_length : 1.2
frequency: 30
initial_state: [0.0,  0.0,  0.0,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0]
use_control : true
control_config: [true, false, false, false, false, true]

imu0: /imu/data/madgwicked
imu0_config: [false, false, false,
              true, true, true,
              false, false, false,
              true, true, true,
              true, true, true]
imu0_differential: false
imu0_relative: false
imu0_queue_size: 7
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true

# imu1: /os_cloud_node/imu
# imu1_config: [false, false, false,
#               false, false, false,
#               false, false, false,
#               true, true, true,
#               true, true, true]
# imu1_differential: false
# imu1_relative: false
# imu1_queue_size: 7
# imu1_pose_rejection_threshold: 0.8
# imu1_twist_rejection_threshold: 0.8
# imu1_linear_acceleration_rejection_threshold: 0.8
# imu1_remove_gravitational_acceleration: true

odom0: /rr100_steering_controller/odom
odom0_config: [false, false, false,
               false, false, false,
               true, true, false,
               false, false, true,
               false, false, false]
odom0_differential: false
odom0_queue_size: 10

odom1: /rtabmap/odom
odom1_config: [true, true, false,
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 10

<launch>
    <!-- Choose visualization -->
    <param name="use_sim_time" type="bool" value="true"/>
    <arg name="rviz"    default="false" />
    <arg name="imu_topic" default="/imu/data/madgwicked" />
    <arg name="lidar_topic" default="/ouster/points" />
    <arg name="gps_topic" default="/ublox_node/fix" />
    <arg name="subscribe_scan_descriptor" default="false"/>
    <arg name="voxel_size"    default="0.5"/>
    <arg name="scan_20_hz"    default="false"/>
    <!-- Point to Plane ICP? -->
    <arg name="p2n" default="false" />
        
    <!--This option feeds wheel odometry to
        icp_odometry as guess ( to be more robust to corridor-like environments).
        If so, use original demo_mapping.bag containing wheel odometry! -->
    <arg name="odom_guess" default="true" />
    <arg name="initial_pose" default="0 0 0 0 0 0" />
    <arg name="localization" value="false"/>
    <arg     if="$(arg localization)" name="rtabmap_args"  default="localization:=true"/>
    <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>

    <!--############################################################ RTABMAP PART#####################################################################-->
    <group ns="rtabmap">  
        <node pkg="rtabmap_ros" name="rtabmap" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
            <remap from="/gps/fix"          to="$(arg gps_topic)"/>
            <remap from="scan_cloud"        to="$(arg lidar_topic)"/>

            <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
            <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
            <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
            <param unless="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="false"/>
            <!--topics related-->
            <param name="queue_size"            type="int"      value="10"/><!--Size of message queue for each synchronized topic-->
            <param name="wait_for_transform"    type="bool"     value="false"/><!-- FIXME : true -->
            <!-- devices -->
            <param name="subscribe_depth"       type="bool"     value="false"/>
            <param name="subscribe_rgb"         type="bool"     value="false"/>
            <param name="subscribe_scan"        type="bool"     value="false"/>
            <param name="subscribe_scan_cloud"  type="bool"     value="true"/>
            <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
            <param name="approx_sync"           type="bool" value="false"/>

            <!-- frames & paths -->
            <param                                  name="publish_tf"            type="bool"     value="false"/><!--Publish TF from /map to /odom-->
            <param                                  name="tf_delay"              type="double"   value="0.05"/><!--Rate at which the TF from /map to /odom is published (20 Hz)-->
            <param                                  name="tf_prefix"             type="string"   value=""/><!--Prefix a ajoute a la tf-->
            <param                                  name="frame_id"              type="string"   value="os_lidar"/>
            <param                                  name="map_frame_id"          type="string"   value="map"/>
            <param                                  name="odom_frame_id"         type="string"   value="odom"/>
            <param if=    "$(arg localization)"     name="database_path"         type="string"   value="~/.ros/rtabmap_clean.db"/>
            <param unless="$(arg localization)"     name="database_path"         type="string"   value="~/.ros/rtabmap.db"/>

            <!--mapping-->	
            <param name="map_cleanup"               type="bool"     value="false"/><!--efface les nuages de points si aucun subscriber aux nuages-->
            <param name="map_always_update"         type="bool"     value="true"/><!-- refresh grid map even if we are not moving, implique derive quand robot est fixe et que environnement est fixe-->
            <param name="map_empty_ray_tracing"     type="bool"     value="true"/> <!-- fill empty space between the generated scans-->
            
            <param name="Rtabmap/DetectionRate"     type="double"   value="5"/>
            <param name="Rtabmap/LoopGPS"           type="bool"     value="false"/><!--FIXME-->
            <param name="Odom/Holonomic"            type="bool"     value="false"/>
            <param name="Odom/AlignWithGround"      type="bool"     value="true"/>            
            
            <!-- RTAB-Map's parameters -->
            <param name="RGBD/Enabled"                   type="bool"     value="false"/>
            <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/LocalRadius"               type="string" value="2"/>
            <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="Reg/Strategy"                   type="int"    value="1"/><!--0=Vis, 1=Icp, 2=VisIcp-->
            <param name="Reg/Force3DoF"                  type="bool"   value="true"/>
            <param name="Optimizer/GravitySigma"         type="string" value="0.5"/>
            <param name="Optimizer/Strategy"             type="string" value="1"/> 
            <param name="Grid/CellSize"                  type="string" value="0.05"/>
            <param name="Grid/Sensor"                    type="string" value="0"/>
            <param name="Grid/RangeMax"                  type="string" value="0"/>
            <param name="Grid/ClusterRadius"             type="string" value="1"/>
            <param name="Grid/GroundIsObstacle"          type="string" value="false"/>

            <!-- ICP parameters -->
            <param name="Icp/Strategy"              type="int"      value="1"/><!--ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare)-->
            <param name="Icp/CorrespondenceRatio"   type="string"   value="0.1"/>
            <param name="Icp/RangeMax"              type="string"   value="0"/>
            <param name="Icp/MaxRotation"           type="string" value="0.78"/><!--FIXME-->                   
            <param name="Icp/Iterations"          type="string" value="50"/>
            <param name="Icp/VoxelSize"           type="string" value="$(arg voxel_size)"/>
            <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/MaxTranslation"      type="string" value="10"/>
            <param name="Icp/MaxCorrespondenceDistance" type="string" value="3"/>
            <param name="Icp/OutlierRatio"      type="string" value="0.85"/>
            <param name="Icp/Force4DoF"      type="string" value="true"/>
        </node>
    
    <!--############################################################ ICP PART #####################################################################-->
        
        <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
            <remap from="scan_cloud"      to="$(arg lidar_topic)"/>
            <remap from="imu" to="$(arg imu_topic)"/>
            <!--<remap from="odom"      to="/rr00/odometry/filtered"/>-->
            <!--<remap from="odom_info"      to="/rtabmap/odom_info"/>-->
            <param name="scan_voxel_size"       type="double"   value="$(arg voxel_size)"/>
            <param name="subscribe_scan"        type="bool"     value="false"/>
            <param name="subscribe_scan_cloud"  type="bool"     value="true"/>
            <param name="scan_cloud_max_points" type="int"      value="0"/>
            <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"/>

            <param name="publish_tf"            type="bool"     value="false"/>
            <param name="odom_frame_id"         type="string"   value="odom"/>
            <param name="frame_id"              type="string"   value="os_sensor"/><!--os1_sensor_base_link-->
            <param name="deskewing"             type="bool"   value="true"/>
            <param name="initial_pose"          type="string" value="$(arg initial_pose)"/>

            
            <param name="guess_frame_id"   type="string" value="base_link"/>
            <param name="wait_imu_to_init" type="bool" value="true"/>

            <!-- ICP parameters -->
            <param name="Icp/Strategy"              type="int"      value="1"/><!--ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare)-->
            <param name="Icp/CorrespondenceRatio"   type="string"   value="0.2"/>
            <param name="Icp/RangeMax"              type="string"   value="0"/>
            <param name="Icp/Force4DoF"             type="string"   value="true"/>
            <param name="Icp/MaxRotation"           type="string"   value="1.5708"/><!--FIXME-->                   
            <param name="Icp/Iterations"          type="string" value="10"/>
            <param name="Icp/VoxelSize"           type="string" value="$(arg voxel_size)"/>
            <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/MaxTranslation"      type="string" value="5"/>
            <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
            <param name="Icp/OutlierRatio"      type="string" value="0.1"/>
            <!--<param if="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="true"/>
            <param if="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
            <param if="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
            <param unless="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
            <param unless="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
            <param unless="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0"/>-->

            <!-- Odom parameters -->
            <param name="Odom/Strategy"        type="string" value="0"/><!--"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");--> <!-- http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-td8261.html#a8563-->
            <param name="Odom/FilteringStrategy" type="int"  value="1"/><!--0=No filtering 1=Kalman filtering 2=Particle filtering-->
            <param name="Odom/GuessMotion"     type="string" value="true"/><!-- can't guess transform from null transform-->
            <param name="Odom/ResetCountdown"  type="string" value="0"/>
            <param name="Odom/ScanKeyFrameThr"  type="string" value="0.9"/><!-- was 0.9 : [Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame."-->
            <param name="Odom/Holonomic"            type="bool"     value="false"/>
            <param name="Odom/AlignWithGround"      type="bool"     value="true"/>

            <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
            <param name="OdomF2M/ScanMaxSize"        type="string" value="0"/>
            <param name="OdomLOAM/AngVar2"      value="0.01"/><!--[Angular output variance.]-->                            
            <param name="OdomLOAM/LinVar"       value= "0.01"/><!--[Linear output variance.]-->
            <param name="OdomLOAM/LocalMapping" value="true"/><!--[Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.]-->
            <param name="OdomLOAM/Resolution"   value="0.2"/><!--[Map resolution]-->
            <param name="OdomLOAM/ScanPeriod"   value="0.2"/><!--[Scan period (s)]-->
            <param name="OdomLOAM/Sensor"       value="2"/><!--[Velodyne sensor: 0=VLP-16, 1=HDL-32, 2=HDL-64E]-->
        </node>
    </group>

    <!-- Visualisation RVIZ -->
    <!--<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d /home/axel/.rviz/config.rviz" output="screen"/>-->

</launch>

 EDIT : forgot to mention that my robot is outside on a parking lot so I took Voxel_size = 0.5 as you advised to.

Another point related to this post is that I am receiving 0 using Floam and crash with Loam. I did not tried on the Kitti benchmark but I think it is not related with my inputs.
Cheers,
Axel

[matlabbe]

Hi,

set only scan_voxel_size parameter. In the warning, voxel=0.05 not 0.5 (maybe it is a warning not coming from odometry but another node, check with rqt_console). With range up to 100 meters, at 5 cm there could be indeed overflow. Outdoor (with long range lidars) I generally set voxel size between 0.3 and 0.5.

Not sure to understand this comment:

- merge WM clouds, neither using rtabmap with use_scan_clouds or use_scan_descriptor from cmr_lidarloop. I do not know where to look into : either robot_localization or rtabmap.

but in your launch, if you use subscribe_scan_descriptor, you should remap scan_descriptor topic too.

Another point related to this post is that I am receiving 0 using Floam and crash with Loam.

I would need your system configuration, which repos you used exactly and commit version of those repos.

cheers,
Mathieu

[Bracky]

Hello Matthieu,

matlabbe wrote

Hi,

set only scan_voxel_size parameter. In the warning, voxel=0.05 not 0.5 (maybe it is a warning not coming from odometry but another node, check with rqt_console). With range up to 100 meters, at 5 cm there could be indeed overflow. Outdoor (with long range lidars) I generally set voxel size between 0.3 and 0.5.

So I tried filtering the distance (20m) and replaced my ICP/VoxelSize with scan_voxel_size but still have the warning issue and the size being 0.05 instead of 0.5.

[ WARN] (2023-01-16 10:28:00.561) util3d_filtering.cpp:539::voxelizeImpl() Leaf size is too small for the input dataset. Integer indices would overflow. We will split space to be able to voxelize (lvl=0 cloud=131072 min=[-109.331596 -99.119019 -1.479200] max=[77.939636 46.475895 15.413268] voxel=0.050000).

However, this message does not appear in rqt_console (tools that I did not know the existence of, thanks for the tip !!) .

Not sure to understand this comment:

- merge WM clouds, neither using rtabmap with use_scan_clouds or use_scan_descriptor from cmr_lidarloop. I do not know where to look into : either robot_localization or rtabmap.

but in your launch, if you use subscribe_scan_descriptor, you should remap scan_descriptor topic too.

Yes I have done the remap (in cmr_lidarloop) but my point is that whether I use cmr_lidarloop or not, I always have the same issue which is that my point cloud remains in the working memory and that there is nothing in the local map :

[ INFO] [1673858543.918015586, 1673343087.419828279]: Assembled 1 obstacle and 1 ground clouds (59294 points, 0.004693s)
[ INFO] [1673858543.938769094, 1673343087.419828279]: rtabmap (9): Rate=0.20s, Limit=0.000s, Conversion=0.0018s, RTAB-Map=1.4808s, Maps update=1.4485s pub=0.0254s (local map=0, WM=9)

My bet is that I have a poorly tuned parameter. But maybe it is because the odometry is not so good. I used gps+ imu + rtamap in my robot_localization setup.
I Actually do not know the proper behavior of the tf map->odom but it is shaking and drifting and I hoped the lidar odometry to correct that. I do not know if the odom is poor because of the bad results from icp or if icp cannot provides good results because of crappy odom.



The point cloud is fine, but the occupancy grid etc are not. Is it because of an error in map -> odom ? Do you have clues where I should look into ?

Another point related to this post is that I am receiving 0 using Floam and crash with Loam.

I would need your system configuration, which repos you used exactly and commit version of those repos.

cheers,
Mathieu

So I am on Ubuntu 20.04, ros noetic, rtabmap 0.20.22 and the latest rtabmap_ros. For the floam I did the following :

cd ~/catkin_ws/src 
git clone https://github.com/wh200720041/floam && \
    cd floam && \
    wget https://gist.githubusercontent.com/matlabbe/d8ab56e69146c18afbd1a3b05444c649/raw/f1f37ece5d5787c77f008cb89e9446fc20a40c1f/floam_as_library_support.patch && \
    git apply floam_as_library_support.patch
cd ../..
catkin_make

Thank you as always for your availability and help ;)

UPDATE : changed for point to plane -> solved most of my pbm

[matlabbe]

Could you share a resulting database?

[Bracky]

good or bad one ?

[matlabbe]

The bad one to see problems. You can also share a good one to compare.

[Bracky]

I finnaly got to make my issue away with

<param name="Odom/Holonomic"            type="bool"     value="true"/><!-- keep true or issues -->
<param name="Odom/AlignWithGround"      type="bool"     value="false"/><!--keep false-->

However I get errors (of course..) from time to time :

[FATAL] (2023-01-18 11:23:52.798) Optimizer.cpp:198::getConnectedGraph() Condition (uContains(posesIn, fromId)) not met!
[FATAL] [1674037432.798395719]: [FATAL] (2023-01-18 11:23:52.798) Optimizer.cpp:198::getConnectedGraph() Condition (uContains(posesIn, fromId)) not met!
[ERROR] [1674037432.798550250]: Exception thrown while processing service call: [FATAL] (2023-01-18 11:23:52.798) Optimizer.cpp:198::getConnectedGraph() Condition (uContains(posesIn, fromId)) not met!
[ERROR] [1674037432.798738202]: Service call failed: service [/rtabmap/add_link] responded with an error: [FATAL] (2023-01-18 11:23:52.798) Optimizer.cpp:198::getConnectedGraph() Condition (uContains(posesIn, fromId)) not met!
[ERROR] [1674037432.798793885]: Failed to call /rtabmap/add_link service

But it also mostly works :

[ INFO] [1674037443.458105606]: rtabmap (158): Rate=1.00s, Limit=0.000s, Conversion=0.0046s, RTAB-Map=0.7958s, Maps update=0.0022s pub=0.0000s (local map=134, WM=134)
[ INFO] [1674037443.460892161]: Adding external link 156 -> 145
[ INFO] [1674037443.494630111]: Odom: ratio=0.436831, std dev=0.094351m|0.029837rad, update time=0.083330s
[ INFO] [1674037443.531810001]: rtabmap: Getting node data (1 node(s), images=false scan=true grid=false user_data=false)...
[ WARN] [1674037443.556314282]: Path for saving the point clouds is not a folder. Scans are not saved.
[ INFO] [1674037443.669526327]: rtabmap: Getting node data (1 node(s), images=false scan=true grid=false user_data=false)...
[ INFO] [1674037443.849866910]: Odom: ratio=0.433659, std dev=0.094810m|0.029981rad, update time=0.329669s
[ INFO] [1674037443.975003725]: Odom: ratio=0.442855, std dev=0.095170m|0.030095rad, update time=0.100286s
[ INFO] [1674037444.111455683]: Odom: ratio=0.440176, std dev=0.095499m|0.030199rad, update time=0.098759s
[ INFO] [1674037444.282366337]: Odom: ratio=0.439443, std dev=0.095807m|0.030297rad, update time=0.140219s
[ INFO] [1674037444.408549581]: Odom: ratio=0.436242, std dev=0.094072m|0.029748rad, update time=0.095668s
[ INFO] [1674037444.569299301]: Odom: ratio=0.436829, std dev=0.095291m|0.030134rad, update time=0.131878s
[ WARN] [1674037444.579889414]: Transform from odom to base_footprint was unavailable for the time requested. Using latest instead.

[ INFO] [1674037444.679140985]: Odom: ratio=0.436614, std dev=0.096745m|0.030594rad, update time=0.078610s
[ INFO] [1674037444.801188772]: Odom: ratio=0.436921, std dev=0.095501m|0.030200rad, update time=0.089146s
[ INFO] [1674037444.936520712]: Odom: ratio=0.447515, std dev=0.095891m|0.030323rad, update time=0.107696s
[ INFO] [1674037444.963486292]: rtabmap (159): Rate=1.00s, Limit=0.000s, Conversion=0.0059s, RTAB-Map=0.9807s, Maps update=0.0015s pub=0.0000s (local map=135, WM=135)
[ INFO] [1674037444.998408226]: Adding external link 157 -> 146
[ INFO] [1674037445.053921023]: Odom: ratio=0.442909, std dev=0.096624m|0.030555rad, update time=0.087345s
[ INFO] [1674037445.167331817]: Odom: ratio=0.437774, std dev=0.095377m|0.030161rad, update time=0.087631s
[ INFO] [1674037445.280415109]: Odom: ratio=0.436815, std dev=0.093472m|0.029559rad, update time=0.083295s
[ INFO] [1674037445.411078768]: Odom: ratio=0.439223, std dev=0.093751m|0.029647rad, update time=0.100422s
[ INFO] [1674037445.543368550]: Odom: ratio=0.431603, std dev=0.095126m|0.030082rad, update time=0.104199s
[ INFO] [1674037445.666114477]: Odom: ratio=0.441509, std dev=0.095763m|0.030283rad, update time=0.097233s
[ INFO] [1674037445.788450293]: Odom: ratio=0.438848, std dev=0.095244m|0.030119rad, update time=0.087815s
[ INFO] [1674037445.904624325]: rtabmap (160): Rate=1.00s, Limit=0.000s, Conversion=0.0041s, RTAB-Map=0.8446s, Maps update=0.0016s pub=0.0000s (local map=136, WM=136)
[ INFO] [1674037445.904850270]: rtabmap: Getting node data (1 node(s), images=false scan=true grid=false user_data=false)...

However I am forced to publish the tf of icp_odometry

<param name="publish_tf"            type="bool"     value="true"/>

because idk why the robot_localization package provides a wrong odometry with the above parameters. I built it as follow :

I'll try to update with a link to a rosbag, but it's snowing rn and I did not recorded one on the former test of rtabmap. Idk if you assembled both packages before, but my issue mostly comme from the poor link I've made between both packages and mistakes on tunning parameters either on rtabmap or robot_localization.
See :
test_all.launch
cmr_lidarloop_params.yaml
localization_global.yaml
 or
localization_global.yaml
localization_local.yaml


If you have a rosbag to try it on, the :
- imu driver output topic is /imu/data
- gps => /ublox_node/fix
- lidar => /ouster/points

[matlabbe]

I don't have a rosbag to test with. Looking at the error, if you relaunch with "--udebug" in rtabmap's node args, we could get more log to debug this. It seems crashing when you add a new external link.