What parameters should I use to run the ros demo "3.1 Robot mapping"? LiDAR ICP doesn't seem to work as good as I expect

[wilsonthongwk]

Dear matlabbe,

Greeting! and I am new to this forum.

I have a setup using realsense D435i with Turtlebot3. I am try to do 3D mapping with D435 camera + LiDAR in my office corridor. The turtlebot3 is going a straight line path for about 10 meters, turn 180degree, come back to where it was.

I am expecting the LiDAR-based ICP can help "sticking" the forward-view and backward-view nicely, but it doesn't seem to work.
 
The following screenshot shows these problems
1) the same office partition is not aligned together between the "forward-view" and "backward-view",
2) rtabmap does not add "ICP edge" to the graph between the forward-view-nodes and back-view-nodes



Because of seeing these problem, I go back to run again the robot mapping demo in http://wiki.ros.org/rtabmap_ros#Robot_mapping.

But still I can't get a good results. See some screenshots below.




What's wrong did I do? am I using some incorrect parameters?

I am using the config parameters in the following file to produce the turtlebot3 results and demo results. The max depth is set to 2.0M for mapping, as depth from D435 is too noisy!

config-rtabmap-demo.ini

I also tried clicking the "restore defaults" button to the default parameters but the forward-view and backward-view still cannot be aligned properly. These default parameters are in the following config file.

config-default.ini

Very much appreciated if you would give me some tips for me to test and improve the LiDAR-ICP results.

Thanks a lot!
Wilson

[matlabbe]

Can you share the resulting database? It will be a lot easier to comment.

[wilsonthongwk]

Thanks matlabbe for your reply. Finally I have fixed my issue.

When my robot is turning back 180 degree, the rtabmp log shows that ICP registration begins to run, but the match ratio needs to be as good as 0.8 or more. My robot usually can only produce laser match with correspondence ratio at 0.5 or 0.6.

[ INFO] [1609378276.699829539]: rtabmap (126): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0770s, Maps update=0.0011s pub=0.0000s (local map=116, WM=116)
[ INFO] (2020-12-31 01:31:17.706) Rtabmap.cpp:1059::process() getting data...
[ INFO] (2020-12-31 01:31:17.706) Rtabmap.cpp:1205::process() Updating memory...
[ INFO] (2020-12-31 01:31:17.706) Memory.cpp:4311::createSignature() Extract features
[ INFO] (2020-12-31 01:31:17.764) Rtabmap.cpp:1228::process() Processing signature 127 w=0 map=0
[ INFO] (2020-12-31 01:31:17.764) Rtabmap.cpp:1230::process() timeMemoryUpdate=0.058916s
[ INFO] (2020-12-31 01:31:17.765) Rtabmap.cpp:1323::process() Odometry refining: guess = xyz=0.450933,-0.000002,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] (2020-12-31 01:31:17.771) Rtabmap.cpp:1334::process() Odometry refining: update neighbor link (126->127, variance:lin=0.000017, ang=0.000002) from xyz=0.450933,-0.000002,0.000000 rpy=0.000000,-0.000000,0.000000 to xyz=0.461527,-0.006576,0.000000 rpy=0.000000,0.000000,-0.000842
[ INFO] (2020-12-31 01:31:17.771) Rtabmap.cpp:1375::process() timeOdometryRefining=0.006739s
[ INFO] (2020-12-31 01:31:17.771) Rtabmap.cpp:1593::process() timeProximityByTimeDetection=0.000028s
[ INFO] (2020-12-31 01:31:17.771) Rtabmap.cpp:1612::process() computing likelihood...
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:1730::process() timeLikelihoodCalculation=0.000238s
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:1736::process() getting posterior...
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:1741::process() timePosteriorCalculation=0.000197s
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:1752::process() creating hypotheses...
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:1766::process() Highest hypothesis=29, value=0.077528, timeHypothesesCreation=0.000036s
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:1841::process() Time emptying memory trash = 0.004702s,  joining (actual overhead) = 0.000012s
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:2219::process() timeReactivations=0.000007s
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:2430::process() timeProximityBySpaceVisualDetection=0.000190s
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:4261::optimizeGraph() get constraints (ids=10, 10 poses, 9 edges) time 0.000027 s
[ INFO] (2020-12-31 01:31:17.772) OptimizerGTSAM.cpp:592::optimize() Stop optimizing, error is already under epsilon (0.000000 < 0.000010)
[ INFO] (2020-12-31 01:31:17.772) Rtabmap.cpp:4319::optimizeGraph() Optimization time 0.000394 s
[ INFO] (2020-12-31 01:31:17.772) Graph.cpp:1319::radiusPosesFiltering() Cloud filtered In = 10, Out = 2
[ INFO] (2020-12-31 01:31:17.781) RegistrationIcp.cpp:1437::computeTransformationImpl() Cannot compute transform (cor=499 corrRatio=0.692094/0.800000 maxLaserScans=721)
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:2570::process() Local scan matching rejected: Cannot compute transform (cor=499 corrRatio=0.692094/0.800000 maxLaserScans=721)
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:2585::process() timeProximityBySpaceDetection=0.008648s
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:2644::process() timeAddLoopClosureLink=0.000008s
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3135::process() timeMapOptimization=0.000007s
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3162::process() sending stats...
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3174::process() send all stats...
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3295::process() Localization pose = xyz=-6.331108,16.455988,0.000000 rpy=0.000000,-0.000000,0.014188
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3299::process() Set map correction = xyz=-1.552748,-0.795969,0.000000 rpy=0.000000,0.000000,-0.115400
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3372::process() Time creating stats = 0.000147...
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3452::process() timeMemoryCleanup = 0.000112s... 0 signatures removed
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3465::process() Total time processing = 0.075298s...
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3595::process() Time limit reached processing = 0.000020...
[ INFO] (2020-12-31 01:31:17.781) Rtabmap.cpp:3639::process() Adding data 127 [0] (rgb/left=1 depth/right=1)
[ INFO] [1609378277.782946752]: rtabmap (127): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0761s, Maps update=0.0009s pub=0.0000s (local map=117, WM=117)

The "Icp/CorrespondenceRatio" is set to 0.4 in the demo_robot_mapping.launch script, and this threshold is doubled during rtabmap initialisation.

So I change the "Icp/CorrespondenceRatio" from 0.4 to 0.2 in the demo_robot_mapping.launch script, and my issue is fixed. Now I can generate many yellow edges between forward-looking and backward-looking keyframes in the pose-graph.



Here is the resulting map database file for your reference, https://drive.google.com/file/d/1mVKfgM4W7is5yfxuPhTWvr3YMVdU_kOS/view?usp=sharing

Thank you very much!
Wilson

[matlabbe]

Hi,

Glad you made it work. I've made the changes in the demo launch file for the ratio.

cheers,
Mathieu