Question on ICP Frequency and libpointmatcher outlierFilter

[seanxu112]

Hi Mathieu,

I am using a very cheap 2d lidar YDlidar TX20 

The problem we are having is that the lidar has low angular resolution and low scanning frequency; we are also navigating a complex environment. Sometimes the lidar does not catch some features in the environment (due to low angular resolution), and it seems like the icp would shift a bit. I will upload an image of this issue later.

My attempted solution of this problem is use libpointmatcher to filter outliers; I have only applied an outlier filter, haven't touched too much of the parameter.

Before I read more into libpointmatcher and optimizes the outlier filter, my question is

1. Is there any libpointmatcher setting you would recommend in this case?

2. How fast is rtabmap running icp? Is this based on the detection rate, or is it based on the frequency of the lidar scan topic frequency. I can see if it is based on the detection rate, by increase the icp frequency (icp iteration based on how fast the scan is coming in), the localization would be better.

Sincerely,

Sean Xu

Edit: This is a snapshot of the map and the scan, a lot of the areas are just chairs and poles; the grid map cleared it as some ray tracing decreased the probability of occupancy. Would the icp problem actually be solved by changing the grid probability during mapping?



I guess one of the problem I always had is that, is the icp done on the gridmap, or the scans from previous nodes?

[matlabbe]

Hi,

1. You can decrease Icp/PMOutlierRatio parameter to be more robust to noise and dynamic points.

2. If you are using icp_odometry, it is done as fast as it can, up to lidar frame rate. On rtabmap side, map is updated by Rtabmap/DetectionRate parameter (default 1 Hz). Having higher frequency will give better odometry.

The scan matching is done against previous raw scans, not the grid map.

cheers,
Mathieu

[seanxu112]

Oh I see. I will try out the PMOutlierRatio, will keep this updated.

I am using t265 as the odometry source, so that means the icp is running at the detection rate. When you say icp is done against previous raw scans, does that mean every scan in the db, or scans from the closest node?

Cheers,
Sean

[seanxu112]

It turns out since the angular resolution is pretty low, and I was applying a
  - SurfaceNormalDataPointsFilter:
        knn: 10

The Icp gives out a bad guess, especially I am running the robot at a space where scans might be sparse.

I also set the DetectionRate up to 4 Hz, and the terminal did output the "Rate = 0.25s", is there a way to check if rtabmap is actually running at 4 Hz (specifically libpointmatcher is running at 4Hz)? Sometimes I feel like the t265 odometry drifted a bit, since I can see the scan mismatch on rviz; it takes a couple of seconds for libpointmatcher to correct for it. When ICP corrects it looks good, but that delay makes me wonder if ICP is actually running at 4 Hz.

Cheers,
Sean

[seanxu112]

I think I found the issue:

rtabmap (1528): Rate=0.25s, Limit=0.000s, RTAB-Map=3.3742s, Maps update=0.0230s pub=0.0000s (local map=1081, WM=1081)

This means rtabmap's real processing rate is 1/3 Hz correct?

Things I have tried:
1. Connect rtabmap and rgbd_sync node to realsense camera manager nodelet (The improvement seems to be negligible)
2. Lower the resolution of the d435 camera to 240p (The processing rate raised to around 2Hz)

Then I checked if my opencv version, it was not compiled with cuda support, I am wondering if that would be the biggest bottleneck. However none of the 6 cores are running at 100%, the average is around 65% while one of the may hit up to 85%.

Do you have any idea which solution would be best or is there anything else I should be doing but not doing?

My launch files are attached:
rtabmap.launch
rtabmap_localization.launch

Cheers,
Sean

[matlabbe]

The Rate is the target rate, however RTAB-Map uses 3.3 seconds in your case for each update, thus running 1/3 Hz as you said.

You would have to check which part rtabmap is taking most of the time, so it can be optimized for your case. You can open the resulting map in rtabmap-databaseViewer and open Statistics panel, then show timing results.

ICP corrections can be done at two levels in rtabmap node:
1) RGBD/NeighborLinkRefining: ICP is done with previous node added (used to correct odometry from previous pose).
2) RGBD/ProximityBySpace: done by assembling scans around current position to find local loop closures. See Figure 5 of this paper.

Note also tha Kp/DetectorStrategy and Vis/FeatureType should generally be the same, and they should have the same values during localization. Kp/DetectorStrategy=1 means SIFT, which is computensively expensive depending on your computer.

[seanxu112]

Thank you for the detailed response. I will first check out the statistics and if sift is the bottleneck I will either switch out the type or see if compile openCV with cuda will work.

Will keep you updated.

Cheers,
Sean

[seanxu112]

It turns out Memory update is taking 2682 ms, where the total is 2687 ms. Is this due to us having 1500 nodes in the db (About 2GB)? And is there a way to resolve this without limiting Mem/MemoryThr? If not I will update this problem: http://official-rtab-map-forum.206.s1.nabble.com/MemoryThr-parameter-messes-up-the-global-costmap-td6826.html since it is the problem we encountered last time. And I abandoned that approach back then.

Sincerely,
Sean

Edit: Turns out it is indeed SIFT, probably need to compile opencv again to see how to make it faster. I'm still not sure what Memory update is indicating.

[matlabbe]

Hi,

can you run this on your database:
EDIT: added "--export" option to export the statistic file:

rtabmap-report TimingMem/Add_new_words/ms TimingMem/Compressing_data/ms TimingMem/Descriptors_extraction/ms TimingMem/Joining_dictionary_update/ms TimingMem/Keypoints_3D/ms TimingMem/Keypoints_detection/ms TimingMem/Occupancy_grid/ms Timing/Likelihood_computation/ms Timing/Map_optimization/ms Timing/Proximity_by_space/ms Timing/Posterior_computation/ms Timing/Add_loop_closure_link/ms --export ~/.ros/rtabmap.db

This will show a graph like this:


Also

$ rtabmap --version
$ rtabmap-info --diff ~/.ros/rtabmap.db

cheers,
Mathieu

[seanxu112]

The image above is the map I created with Sift.


The image above is the map I created with orb.

output_version_info.txt
The above is the output of rtabmap --version and rtabmap-info.

After I realized sift was the problem, I switched to orb, and it seems like it is running much faster. Now from the graph indeed it is running faster. With Orb it seems like the biggest chunk is optimization, which makes sense as the map grows larger and larger. Is this computation running in localization mode?

I am currently running these commands not on the robot, so the rtabmap versions might not be correct. I also only ran the rtabmap-info on the sift db. Let me know if this is enough.

Thank you for looking these over.

Cheers,
Sean

[matlabbe]

Hi,

It is indeed SIFT keypoint detection and descriptor extraction (total>2 seconds per image) that are using a lot of time. Using binary features like ORB can indeed be a lot faster to detect and extract.

For graph optimization, it is indeed increasing as the graph is growing. Note that in localization mode, the Map Optimization would stay at 0.

To limit the time for map optimization, we should enable memory management of RTAB-Map, which is not trivial for newcomers. See figure 18 of this paper and read what are the consequences of enabling memory management (also this paper for navigation issues).

cheers,
Mathieu

[seanxu112]

I see. I have indeed set both MemoryThr and TimeThr both to 0. And I am using ORB for now.

What would be the difference between setting the parameters above vs record the same mapping session (maybe a bit longer since the processing time would increase) and post-process the database without memory management?

I will try enabling MemoryThr and TimeThr next time when I map a large area.

Cheers,
Sean

[matlabbe]

It depends what is your goal. If you are fine with recording the data and do the mapping offline, you can post-process without memory management.

If you need a map online, you could do SLAM online with memory management enabled (use RGBD/OptimizeFromGraphEnd=true to avoid jumping while mapping when old nodes a transferred in Long-Term Memory). Then if you need the full global map in localization, you can reprocess that database without memory management parameters (or even different visual features Kp/DetectorStrategy):

rtabmap-reprocess --Rtabmap/MemoryThr 0 --Rtabmap/TimeThr 0 online_map.db offline_map.db 

cheers,
Mathieu