Gaining understanding in the many different filter parameters and options

[Rik]

Hello,

I am trying to gain understanding in the many different filter option that exist within icp_odometry and rtabmap, as we are trying to improve the density of our final outputs cloud_map and ocotmap. I started investigating the source code of icp_odometry, but noticed a few strange things.

Our setup uses two 3d lidars that have their pointclouds merged using rtabmap_ros/point_cloud_aggregator.

To sketch the confusion I have, I listed the parameters related to filtering we currently use:
scan_voxel_size: 0.2
Grid/CellSize: 0.05
Icp/VoxelSize: 0.3
OdomF2M/ScanSubtractRadius: 0.5

and the warnings I get

[ WARN] [1659084452.098338173]: IcpOdometry: Both parameter "Icp/VoxelSize" and ros parameter "scan_voxel_size" are set.
[ WARN] [1659084452.435670922]: Parameter "scan_voxel_size" has moved from rtabmap_ros to rtabmap library. Use parameter "Grid/CellSize" instead. The value is still copied to new parameter name.

The first warning I found in https://github.com/introlab/rtabmap_ros/blob/master/src/nodelets/icp_odometry.cpp#L217-L239. In our case we have set both parameters to different values. For icp_odometry.cpp this means that scan_voxel_size (internal variable: scanVoxelSize_) is used in several voxalize steps.

Q1: Does this mean that we filter twice with different values? Once in icp_odometry.cpp (rtabmap_ros) via scan_voxel_size and later again in RegistrationIcp.cpp (rtabmap) using Icp/VoxelSize for commonFiltering()?
Q2: In some cases of the conditional statements in https://github.com/introlab/rtabmap_ros/blob/master/src/nodelets/icp_odometry.cpp#L217-L239, the Icp/VoxelSize is set to 0. Is this done to prevent filtering twice?

The second warning confuses me further as it suggests that scan_voxel_size is no longer in use. However, from the previous warning and questions we notice it is still in use. I found this warning in https://github.com/introlab/rtabmap_ros/blob/master/src/MapsManager.cpp#L224-L318. Eventhough, this line (https://github.com/introlab/rtabmap_ros/blob/master/src/MapsManager.cpp#L296) links scan_voxel_size to Grid/CellSize, Grid/CellSize seems to affect a different part of the code, namely, the ocotmap parts in OctoMap.cpp (rtabmap) and OccupancyGrid.cpp (rtabmap).

Q3: Could you clarify if and how the ROS parameter scan_voxel_size has been replaced by Grid/CellSize and how this affects Icp/VoxelSize, as this parameter is also linked to scan_voxel_size?
Q4: Besides these parameters I find other parameters that might do something similar: scan_downsampling_step, Icp/DownsamplingStep, OdomF2M/ScanSubtractRadius, OdomF2M/ScanMaxSize, Grid/PreVoxelFiltering, etc. And at this point I am unable to debug the system as I don't know in which order the filters are applied.

My guess so far for icp_odometry: downsample (using scan_downsampling_step or Icp/DownsamplingStep), voxalize (using scan_voxel_size or Icp/VoxelSize), range filter (using scan_range_min or Icp/RangeMin and scan_range_max or Icp/RangeMax). rtabmap's filters I didn't look at yet.

Q5: As you notice from my guess to Q4's icp_odometry part, there are many duplicate parameters that give rise to a similar questions to Q1. Are filters in some cases applied twice: once in icp_odometry.cpp (ROS layer) and later in the core library during ICP registration? And how about the rtabmap node if we currently load the same settings there, is the filtering then repeated?
Q6: As Grid/RayTracing also functions as a filter on octomap, which we would like to use for dynamic obstacle removal, we wonder how this is done for an aggragated point cloud, which is assembled with two different points of origin. Is this taken into account in the raytracing algorithm or do we unknowingly use an unkown single point of origin?
Q7: As for tuning, I noticed that this example uses the same cell_size for multiple different filter steps, is this recommended and why?

Thank you in advance :)

[matlabbe]

Hi,

Q1: If you explicitly set both "scan_voxel_size" and "Icp/VoxelSize", the scans will be voxelized twice. Set one or the other (set only scan_voxel_size to avoid any warnings).

Q2: Yes, it is there for convenience, if you set only "Icp/VoxelSize", it will be transferred to scan_voxel_size, then set to zero to be ignored down the line (to avoid a second voxel filtering in RegistrationIcp).

Q3: The second warning is about occupancy grid, not icp

Q4: The documentation could be better, though it is an open source project, so it is possible to figure it out. For ICP related stuff, check RegistrationIcp.cpp. The default filtering function is util3d::commonFiltering(). For odometry local scan map (Odom related parameters), see OdometryF2M.cpp. For the Grid stuff, it is not related to ICP odometry, but for rtabmap's occupancy grid mapping. You should see that icp_odometry and rtabmap nodes are two different things and don't share data between them (unless explicitly configured to do so).

Q5: the warnings you see in icp_odometry are there to notice that a double filtering is avoided (unless both parameters for same filter approach are set explicitly). For rtabmap and icp_odometry, they do filter stuff independently form the input point cloud message you connected. If you are ok to use the highly downsampled point clouds from icp_odometry, you can make rtabmap subscribe to output point cloud from icp_odometry (called /rtabmap/odom_filtered_input_scan), then don't set voxel filetring parameters to rtabmap node.

Q6: If you aggregate two scans in base_link frame, the ray tracing will be done from base_link frame, not each lidar's frame. In comparison, as rtabmap supports multiple camera inputs, we do ray tracing from each camera frame, but for lidar this is indeed and issue. We would have to implement a multi-scan/cloud inputs for rtabmap node, so that ray tracing can be done from the sensor frames, and not base_link. This would require some large refactoring effort in both rtabmap and rtabmap_ros projects.

Q7: On icp_odometry side, I would generarly set Icp/VoxelSize and OdomF2M/ScanSubtractRadius to same value. For rtabmap node, Icp/VoxelSize and Grid/CellSize could be often different (do icp registration at 30 cm voxel, but we would want occupancy grid at 5 or 10 cm cell size, depending on your navigation approach).

cheers,
Mathieu