Posted by
Bracky on
Jan 13, 2023; 10:57am
URL: http://official-rtab-map-forum.206.s1.nabble.com/Localization-after-mapping-session-using-lidar-only-tp9260p9288.html
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