Errors on icp odometry

[Mikor]

Hi Mathieu,

I hope you're doing well. I have a set of recorded pointclouds and I am trying to feed them in rtabmap, at first, it did not have any tf message in it so I published one and recorded it into a bag including the pointclouds.

I've built rtabmap from source with libpointmatcher and I run rtabmap.launch with the following arguments.

roslaunch rtabmap_ros rtabmap.launch    \
   use_sim_time:=true \
   depth:=false \
   subscribe_scan_cloud:=true \
   frame_id:=base_link \
   scan_cloud_topic:=/velodyne_points \
   scan_cloud_max_points:=131072  \
   icp_odometry:=true \
   approx_sync:=false \
   args:="-d  \
      --RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 2 \
      --Odom/ScanKeyFrameThr 0.8 \
      --OdomF2M/ScanMaxSize 10000  \
      --OdomF2M/ScanSubtractRadius 0.5   \
      --Icp/PM true \
      --Icp/VoxelSize 0.5   \
      --Icp/MaxTranslation 2   \
      --Icp/MaxCorrespondenceDistance 1.5 \
      --Icp/PMOutlierRatio 0.7 \
      --Icp/Iterations 10 \
      --Icp/PointToPlane true \
      --Icp/PMMatcherKnn 20 \
      --Icp/PMMatcherEpsilon 5 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 5 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01 \
      --Icp/MaxTranslations 5 \
      --Icp/MaxRotations 1.5"

It seems that it registers a few pointclouds at the start and I assume that the odometry drifts away as I get the error that it cannot perform icp registration with a null guess. Also I get a lot of warnings regarding that it detected a jump back in time, I do not know if that is relevant and how to fix this. I attach to this message the bag I am using as it might be useful for debugging purposes.

[matlabbe]

There is something strage with the /velodyne_points in the bag. It is like only the half of the cloud is published at the same time, instead of the full 360. Receiving two consecutive halves without overlap, it is likely that icp_odometry will get lost. To see this, open rviz to show /velodyne_points and set a delay of 0 (to only see last cloud). You will see the cloud flickering. Verify that the velodyne config matches the actual rotating speed set to velodyne.

[Mikor]

Hey Mathieu, thank you for pointing that out I see what you mean. I wonder if there is anything I can do to fix this but probably there is not. Also, what do you mean by saying velodyne config, can you point me somewhere so when I record my own data to avoid this issue, I know it is not a rtabmap related issue but maybe it will help me and other people by seeing this to avoid this issue. Are you refering to the rpm parameter of the VLP16_points.launch file to be set as the rpm which is set at the 1.3 step here?

[matlabbe]

Yes the rpm parameter should be the same between velodyne launch file and the web page of the velodyne.

[Mikor]

Hi Mathieu,

I think I fixed the pointcloud, here is the bag, I attach the full bag this time. It is a scan of a nice building in Barcelona and I did not want it to go to waste. (I merged every two messages of the topic using the point_cloud_assembler.)

Anyway, I am trying to do for starters only icp odometry with it using the following parameters.

$ 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.8 \
      --OdomF2M/ScanMaxSize 10000  \
      --OdomF2M/ScanSubtractRadius 0.5   \
      --Icp/PM true \
      --Icp/VoxelSize 0.5   \
      --Icp/MaxTranslation 2   \
      --Icp/MaxCorrespondenceDistance 1.5 \
      --Icp/PMOutlierRatio 0.7 \
      --Icp/Iterations 10 \
      --Icp/PointToPlane true \
      --Icp/PMMatcherKnn 20 \
      --Icp/PMMatcherEpsilon 5 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 5 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01 \
      --Icp/MaxTranslations 5 \
      --Icp/MaxRotations 1.5"

But it fails miserably, the course of odometry goes wild and finally drifts away. And it makes me thinking how can it go so bad when at most of the kitti datasets I run with the same parameters it worked good enough. Anyway I turned the point to plane off and this odometry is the result.




Also the pointcloud is disappointing. Do you have any suggestions..?

[matlabbe]

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

[Mikor]

Your results are very impressive and thank you for taking the time, even without refinement it is much better than the ones I got. Although when I try to refine the problematic scans I got this error and I do not know how to overcome this.  



Edit: Okay, I had to open the core parameters, and set the reg. strategy to 1.

But I do not get the last part where you said to close the database and save the new link and then open the map in rtab.

[matlabbe]

The last part is just closing rtabmap-databaseViewer, you will be asked to save the link change you've made. You can stay in rtabmap-databaseViewer to export data, but as I did, I reopened the database in rtabmap to visualize the 3D map with the trajectory.

rtabmap my_map.db

Edit: Note that Reg/Strategy should have been already set to 1 with the command I used. Not sure why on your side it was 0 or 2. (with visuel registration)

[Mikor]

Okay, good, that works great! Thank you Mathieu!

[Mikor]

Hey Mathieu,

Sorry for the very late reply but even after many refinements I can not reduce the angular error even under 2, what might be the reason for that..?

[matlabbe]

Not sure which "angular error" you are referring exactly.