Re: Errors on icp odometry

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Errors-on-icp-odometry-tp7546p7643.html

Hi Mikor,

Seeing a cloud like this I would be also disappointed...

Here is what I can get (see below for a description of the issues I had with that bag)





The two commands I used:

$ roslaunch rtabmap_ros rtabmap.launch  \
    use_sim_time:=true \
    depth:=false \
    subscribe_scan_cloud:=true \
    frame_id:=velodyne \
    scan_cloud_topic:=/pointcloud2  \
    scan_cloud_max_points:=131072  \
    icp_odometry:=true  \
    approx_sync:=false \
    args:="-d  \
      --RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 2 \
      --Odom/ScanKeyFrameThr 0.9 \
      --OdomF2M/ScanMaxSize 25000  \
      --OdomF2M/ScanSubtractRadius 0.5   \
      --Icp/PM true \
      --Icp/VoxelSize 0.5   \
      --Icp/MaxTranslation 5   \
      --Icp/MaxCorrespondenceDistance 1.5 \
      --Icp/PMOutlierRatio 0.8 \
      --Icp/Iterations 10 \
      --Icp/PointToPlane true \
      --Icp/PMMatcherKnn 3 \
      --Icp/PMMatcherEpsilon 0 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 10 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01 \
      --Odom/GuessSmoothingDelay 1"

$ rosbag play --clock -s 15 test_50.bag

1) Note the 15 seconds head start into the bag. There is something wrong with the first stamps in the bag, they are like in the future compared to all other stamps (1615522605.259905992 versus 1572616328.735738754), and TF doesn't like this.

2) I noticed also many times velodyne clouds having the same stamp, and odometry is ignoring those clouds, sometime even for 500-600 ms!

[ INFO] [1615522605.259905992, 1615192195.277976594]: Odom: ratio=0.313596, std dev=0.097816m|0.030932rad, update time=0.137488s
[ WARN] [1615522605.307580423, 1615192195.328414022]: Odometry: Detected not valid consecutive stamps (previous=1572616355.361338s new=1572616355.361338s). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.
[ INFO] [1615522605.407457870, 1615192195.419183580]: rtabmap (194): Rate=0.50s, Limit=0.000s, Conversion=0.0045s, RTAB-Map=0.1419s, Maps update=0.0007s pub=0.0000s (local map=180, WM=180)
[ WARN] [1615522605.518284003, 1615192195.530895835]: Odometry: Detected not valid consecutive stamps (previous=1572616355.361338s new=1572616355.361338s). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.
[ WARN] [1615522605.735801403, 1615192195.753975556]: Odometry: Detected not valid consecutive stamps (previous=1572616355.361338s new=1572616355.361338s). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.
[ WARN] [1615522605.926766999, 1615192195.947286034]: Odometry: Detected not valid consecutive stamps (previous=1572616355.361338s new=1572616355.361338s). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.
[ WARN] [1615522606.131799545, 1615192196.152343908]: Odometry: Detected not valid consecutive stamps (previous=1572616355.361338s new=1572616355.361338s). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.
[ INFO] [1615522606.486544615, 1615192196.507006618]: Odom: ratio=0.283657, std dev=0.108447m|0.034294rad, update time=0.146101s

Echoing the point cloud stamps, some are indeed the same:
stamp:
  secs: 1572616328
  nsecs: 735738754
frame_id: "velodyne"
---
seq: 522
stamp:
  secs: 1572616328
  nsecs: 837246418
frame_id: "velodyne"
---
seq: 523
stamp:
  secs: 1572616328
  nsecs: 837246418
frame_id: "velodyne"
---
seq: 524
stamp:
  secs: 1572616328
  nsecs: 837246418
frame_id: "velodyne"
---
seq: 525
stamp:
  secs: 1572616329
  nsecs: 340574265
frame_id: "velodyne"
---
seq: 526
stamp:
  secs: 1572616329
  nsecs: 340574265
frame_id: "velodyne"
---
seq: 527
stamp:
  secs: 1572616329
  nsecs: 340574265
frame_id: "velodyne"
---
seq: 528
stamp:
  secs: 1572616329
  nsecs: 542288780
frame_id: "velodyne"

When skipping clouds, odometry should correct larger gaps between scans, which may make it lose tracking or do wrong registrations. Those stamps above are right where there is a problem during the mapping, when you turned relatively fast the building corner at the same time, ICP could not correctly the rotation at that time missing some important scans.


This one can be corrected with rtabmap-databaseviewer, by browsing the neighbor constraint with the rotation error:


When found, click on Refine a couple of times:


Close the database and save the new link. When opening again the map in rtabmap:


The section of the bridge is more clear now.

3) You can compare my command with yours, you will some tuning there and there...

4) Make sure you have rtabmap built with libpointmatcher support, even with the stamp issues I couldn't get as bad results you had with your original command.

cheers,
Mathieu