Null guess at icp when imu_topic is being used.

[Mikor]

Hello Mathieu,

I'll try to be as detailed as I can to help you help me. My configuration is a VLP-16 and a T265, the data I am using are the following.

types:       nav_msgs/Odometry       [cd5e73d190d741a2f92e81eda573aca7]
             sensor_msgs/Imu         [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
             tf2_msgs/TFMessage      [94810edda583a504dfda3829e70d7eec]
topics:      /realsense_imu_publisher    6392 msgs    : sensor_msgs/Imu        
             /realsense_odom_publisher   6390 msgs    : nav_msgs/Odometry      
             /tf                         4721 msgs    : tf2_msgs/TFMessage     
             /tf_static                   180 msgs    : tf2_msgs/TFMessage     
             /velodyne_points             179 msgs    : sensor_msgs/PointCloud2

About the topics
> /velodyne_points comes from the vlp...
> /tf & tf_static: I publish them using 2 nodes.

rostopic echo /tf_static
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1617965844
        nsecs: 569535970
      frame_id: "imu_link"
    child_frame_id: "velodyne"
    transform: 
      translation: 
        x: 0.0
        y: 0.0
        z: 0.0
      rotation: 
        x: 0.5
        y: -0.5
        z: -0.5
        w: -0.5
---

Being published at 10hz.

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1617965856
        nsecs: 277832984
      frame_id: "odom"
    child_frame_id: "imu_link"
    transform: 
      translation: 
        x: 0.621760427952
        y: -0.0049396622926
        z: 0.440426230431
      rotation: 
        x: -0.0174755975604
        y: -0.975793778896
        z: -0.045290607959
        w: 0.213236629963
---

This is the tf published at 300hz

> The realsense imu topic and the odom topic are taken using the pyrealsense2, imu is a combination of gyro and accel data of t265, the realsense imu topic is published at 350hz and the odom at 360hz.

Tf tree
All these bring us the following tf tree.



Parameters passed to launch file.

roslaunch rtabmap_ros rtabmap.launch       use_sim_time:=true    depth:=false    subscribe_scan_cloud:=true    frame_id:=velodyne    scan_cloud_topic:=/velodyne_points     icp_odometry:=true    approx_sync:=true   guess_frame_id:=odom imu_topic:=/realsense_imu_publisher args:="-d  \
      --RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 0 \
      --Odom/ScanKeyFrameThr 0.8 \
      --OdomF2M/ScanMaxSize 999900000000  \
      --OdomF2M/ScanSubtractRadius 0.05   \
      --Icp/PM true \
      --Icp/VoxelSize 0.05   \
      --Icp/MaxTranslation 2   \
      --Icp/MaxCorrespondenceDistance 1.5 \
      --Icp/PMOutlierRatio 0.7 \
      --Icp/Iterations 150 \
      --Icp/PointToPlane true \
      --Icp/PMMatcherKnn 25 \
      --Icp/PMMatcherIntensity true \
      --Icp/PMMatcherEpsilon 1 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 25 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.1 \
      --RGBD/NeighborLinkRefining true \
      --RGBD/LinearUpdate 0.05"

Errors
This is the output that I get.

[ INFO] [1617972644.356268456]: rtabmapviz started.
[ WARN] (2021-04-09 15:50:49.366) OdometryF2M.cpp:239::computeTransform() Updated initial pose from xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 to xyz=0.000000,0.000000,0.000000 rpy=-1.570697,-0.181515,-1.675437 with IMU orientation
[ INFO] [1617972649.392970834, 1617965844.540460912]: Odom: ratio=0.000000, std dev=99.995000m|99.995000rad, update time=0.025728s
[ INFO] [1617972649.469371307, 1617965844.544113532]: rtabmap (1): Rate=0.00s, Limit=0.000s, Conversion=0.0185s, RTAB-Map=0.0392s, Maps update=0.0129s pub=0.0003s (local map=1, WM=1)
[ WARN] (2021-04-09 15:50:53.280) RegistrationIcp.cpp:1404::computeTransformationImpl() Maximum laser scans points not set for signature 2, correspondences ratio set relative instead of absolute! This message will only appear once.
[ INFO] [1617972653.298409920, 1617965844.735352418]: Odom: ratio=0.968823, std dev=0.004380m|0.001385rad, update time=1.979887s
[ INFO] [1617972653.336351481, 1617965844.737412539]: rtabmap (2): Rate=0.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.0329s, Maps update=0.0011s pub=0.0002s (local map=1, WM=1)
[ INFO] [1617972655.204281931, 1617965844.830787909]: Odom: ratio=0.964299, std dev=0.004602m|0.001455rad, update time=1.825727s
[ INFO] [1617972655.235302357, 1617965844.832348625]: rtabmap (3): Rate=0.00s, Limit=0.000s, Conversion=0.0027s, RTAB-Map=0.0270s, Maps update=0.0008s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972657.597474213, 1617965844.950610395]: Odom: ratio=0.949076, std dev=0.003747m|0.001185rad, update time=2.282081s
[ INFO] [1617972657.640733192, 1617965844.952803186]: rtabmap (4): Rate=0.00s, Limit=0.000s, Conversion=0.0030s, RTAB-Map=0.0389s, Maps update=0.0008s pub=0.0001s (local map=1, WM=1)
[ INFO] [1617972659.290958497, 1617965845.035056548]: Odom: ratio=0.924429, std dev=0.004082m|0.001291rad, update time=1.661706s
[ INFO] [1617972659.323679008, 1617965845.036829207]: rtabmap (5): Rate=0.00s, Limit=0.000s, Conversion=0.0026s, RTAB-Map=0.0288s, Maps update=0.0007s pub=0.0001s (local map=1, WM=1)
[ INFO] [1617972661.112815757, 1617965845.126410082]: Odom: ratio=0.922889, std dev=0.004956m|0.001567rad, update time=1.612965s
[ INFO] [1617972661.155411992, 1617965845.128434903]: rtabmap (6): Rate=0.00s, Limit=0.000s, Conversion=0.0029s, RTAB-Map=0.0365s, Maps update=0.0025s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972663.090630508, 1617965845.225080740]: Odom: ratio=0.924792, std dev=0.004287m|0.001356rad, update time=1.564113s
[ INFO] [1617972663.123340926, 1617965845.226653663]: rtabmap (7): Rate=0.00s, Limit=0.000s, Conversion=0.0029s, RTAB-Map=0.0286s, Maps update=0.0007s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972663.608835556, 1617965845.250874232]: Odom: ratio=0.928779, std dev=0.003766m|0.001191rad, update time=0.172099s
[ INFO] [1617972663.642386300, 1617965845.252946762]: rtabmap (8): Rate=0.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.0291s, Maps update=0.0008s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972666.974353022, 1617965845.419245403]: Odom: ratio=0.933309, std dev=0.004172m|0.001319rad, update time=1.667466s
[ INFO] [1617972667.006879781, 1617965845.420783730]: rtabmap (9): Rate=0.00s, Limit=0.000s, Conversion=0.0030s, RTAB-Map=0.0279s, Maps update=0.0011s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972669.577843745, 1617965845.549493838]: Odom: ratio=0.930953, std dev=0.005013m|0.001585rad, update time=2.108936s
[ INFO] [1617972669.621010709, 1617965845.551523897]: rtabmap (10): Rate=0.00s, Limit=0.000s, Conversion=0.0030s, RTAB-Map=0.0336s, Maps update=0.0059s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972671.186791247, 1617965845.630051862]: Odom: ratio=0.930953, std dev=0.005269m|0.001666rad, update time=1.570845s
[ INFO] [1617972671.222901999, 1617965845.631574948]: rtabmap (11): Rate=0.00s, Limit=0.000s, Conversion=0.0026s, RTAB-Map=0.0323s, Maps update=0.0007s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972673.291825348, 1617965845.735204061]: Odom: ratio=0.932131, std dev=0.004752m|0.001503rad, update time=1.780018s
[ INFO] [1617972673.334336211, 1617965845.737426126]: rtabmap (12): Rate=0.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.0375s, Maps update=0.0012s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972675.152798953, 1617965845.828446471]: Odom: ratio=0.933037, std dev=0.005808m|0.001837rad, update time=1.579627s
[ INFO] [1617972675.186240806, 1617965845.829962152]: rtabmap (13): Rate=0.00s, Limit=0.000s, Conversion=0.0030s, RTAB-Map=0.0289s, Maps update=0.0009s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972677.283975902, 1617965845.934789068]: Odom: ratio=0.937115, std dev=0.005949m|0.001881rad, update time=1.819435s
[ INFO] [1617972677.326754674, 1617965845.936813139]: rtabmap (14): Rate=0.00s, Limit=0.000s, Conversion=0.0029s, RTAB-Map=0.0384s, Maps update=0.0009s pub=0.0001s (local map=1, WM=1)
[ INFO] [1617972679.209125420, 1617965846.031005881]: Odom: ratio=0.932403, std dev=0.006666m|0.002108rad, update time=1.657065s
[ INFO] [1617972679.248684282, 1617965846.033135750]: rtabmap (15): Rate=0.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.0331s, Maps update=0.0024s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972679.688637619, 1617965846.054909548]: Odom: ratio=0.940558, std dev=0.007974m|0.002522rad, update time=0.226223s
[ WARN] (2021-04-09 15:51:19.692) Transform.cpp:513::getTransform() No transform found for stamp 1617965846.032350! Latest is 1617965846.029031
[ WARN] [1617972679.692149906, 1617965846.055424758]: We are receiving imu data (buffer=564), but cannot interpolate imu transform at time 1617965846.032350. IMU won't be added to graph.
[ INFO] [1617972679.721928122, 1617965846.056515720]: rtabmap (16): Rate=0.00s, Limit=0.000s, Conversion=0.0030s, RTAB-Map=0.0291s, Maps update=0.0007s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972683.242045511, 1617965846.232641607]: Odom: ratio=0.949801, std dev=0.006672m|0.002110rad, update time=1.789451s
[ INFO] [1617972683.276176895, 1617965846.234197333]: rtabmap (17): Rate=0.00s, Limit=0.000s, Conversion=0.0031s, RTAB-Map=0.0295s, Maps update=0.0010s pub=0.0001s (local map=1, WM=1)
[ INFO] [1617972685.331040355, 1617965846.337048410]: Odom: ratio=0.932584, std dev=0.007452m|0.002356rad, update time=1.577450s
[ INFO] [1617972685.382709171, 1617965846.339622877]: rtabmap (18): Rate=0.00s, Limit=0.000s, Conversion=0.0030s, RTAB-Map=0.0473s, Maps update=0.0006s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972687.241066464, 1617965846.432882271]: Odom: ratio=0.925788, std dev=0.007531m|0.002382rad, update time=1.724877s
[ INFO] [1617972687.278201997, 1617965846.434405047]: rtabmap (19): Rate=0.00s, Limit=0.000s, Conversion=0.0033s, RTAB-Map=0.0303s, Maps update=0.0006s pub=0.0001s (local map=1, WM=1)
[ INFO] [1617972689.487696247, 1617965846.544810482]: Odom: ratio=0.922798, std dev=0.007685m|0.002430rad, update time=1.804963s
[ INFO] [1617972689.530619041, 1617965846.547371009]: rtabmap (20): Rate=0.00s, Limit=0.000s, Conversion=0.0028s, RTAB-Map=0.0386s, Maps update=0.0010s pub=0.0001s (local map=1, WM=1)
[ INFO] [1617972691.597110676, 1617965846.650700753]: Odom: ratio=0.925607, std dev=0.008105m|0.002563rad, update time=1.798482s
[ INFO] [1617972691.646192172, 1617965846.652729831]: rtabmap (21): Rate=0.00s, Limit=0.000s, Conversion=0.0092s, RTAB-Map=0.0387s, Maps update=0.0006s pub=0.0000s (local map=1, WM=1)
[ INFO] [1617972693.229490415, 1617965846.731978155]: Odom: ratio=0.910475, std dev=0.008368m|0.002646rad, update time=1.599814s
[ INFO] [1617972694.378623174, 1617965846.789533013]: Odom: ratio=0.904494, std dev=0.008495m|0.002686rad, update time=0.496291s
[ WARN] (2021-04-09 15:51:35.040) RegistrationIcp.cpp:1404::computeTransformationImpl() Maximum laser scans points not set for signature 22, correspondences ratio set relative instead of absolute! This message will only appear once.
[ INFO] [1617972695.042482788, 1617965846.822851609]: rtabmap (22): Rate=0.00s, Limit=0.000s, Conversion=0.0026s, RTAB-Map=1.8092s, Maps update=0.0006s pub=0.0000s (local map=2, WM=2)
[ INFO] [1617972695.172023509, 1617965846.829196128]: rtabmap (23): Rate=0.00s, Limit=0.000s, Conversion=0.0031s, RTAB-Map=0.1227s, Maps update=0.0007s pub=0.0000s (local map=3, WM=3)
[ INFO] [1617972697.138524013, 1617965846.927325126]: Odom: ratio=0.915730, std dev=0.008884m|0.002809rad, update time=1.566809s
[ INFO] [1617972697.237275101, 1617965846.932426895]: rtabmap (24): Rate=0.00s, Limit=0.000s, Conversion=0.0027s, RTAB-Map=0.0945s, Maps update=0.0007s pub=0.0001s (local map=4, WM=4)
[ INFO] [1617972698.000217879, 1617965846.970533384]: Odom: ratio=0.917185, std dev=0.008238m|0.002605rad, update time=0.283851s
[ INFO] [1617972698.043134303, 1617965846.972586429]: rtabmap (25): Rate=0.00s, Limit=0.000s, Conversion=0.0034s, RTAB-Map=0.0380s, Maps update=0.0007s pub=0.0000s (local map=4, WM=4)
[ INFO] [1617972699.918961609, 1617965847.066564842]: Odom: ratio=0.925564, std dev=0.008328m|0.002634rad, update time=0.298617s
[ INFO] [1617972699.956728982, 1617965847.068626803]: rtabmap (26): Rate=0.00s, Limit=0.000s, Conversion=0.0031s, RTAB-Map=0.0333s, Maps update=0.0008s pub=0.0000s (local map=4, WM=4)
[ERROR] (2021-04-09 15:51:41.631) RegistrationIcp.cpp:1475::computeTransformationImpl() RegistrationIcp cannot do registration with a null guess.
[ WARN] (2021-04-09 15:51:41.631) OdometryF2M.cpp:557::computeTransform() Registration failed: "RegistrationIcp cannot do registration with a null guess."
[ INFO] [1617972701.649660316, 1617965847.152715350]: Odom: ratio=0.000000, std dev=0.000000m|0.000000rad, update time=0.018491s
[ERROR] (2021-04-09 15:51:41.653) Rtabmap.cpp:1148::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 26 is ignored!
[ INFO] [1617972701.653824877, 1617965847.153234841]: rtabmap (26): Rate=0.00s, Limit=0.000s, Conversion=0.0029s, RTAB-Map=0.0003s, Maps update=0.0000s pub=0.0000s (local map=4, WM=4)

Soon enough, the odometry is lost, and I wonder what might be the reason, without the imu_topic everything runs smoothly but I am very interested in using the imu_topic as well. I noticed that the drift happens soon after adding a second node to the graph.

Bonus question: what is the white line in rtabmap close to the trajectory ?

[Mikor]

Also I noticed this abnormal behavior:



And I have no idea why is that happening, I run the bag at 0.05 rate so I do not know if this is a performance issue, but sometimes when I had similar results I restarted the VM and it worked fine but know this persists... I dont know why or what to do about it.

Also setting wait_imu_to_init to true did the job and solved the previously mentioned issues.

[matlabbe]

Hi,

Is the TF tree you shown is the bag only or while running rtabmap at the same time? The Tf map->odom should not be in the bag.

I would add "odom_frame_id:=icp_odom" to your rtabmap line, to make sure TF are correctly published, otherwise if multiple frames are published over odom frame, it may cause jumps like you saw.

The white line is the odometry poses, the blue line is the map's graph. The white line may jump when the map is optimized.

EDIT: don't set wait_imu_to_init when using guess. The guess would already include imu. I woould have to doublecheck as imu should be ignored if odom guess is used for icp_odometry node. rtabmap will still subscribe to imu for graph optimization.

cheers,
Mathieu

[Mikor]

The tf that there is in the bag is odom->imu_link->velodyne.

If I do what you suggested, as is, I get the following tree:


And here is the launch command:

roslaunch rtabmap_ros rtabmap.launch       use_sim_time:=true \
depth:=false \
subscribe_scan_cloud:=true \
frame_id:=velodyne \
scan_cloud_topic:=/velodyne_points  \
icp_odometry:=true \
approx_sync:=true  \
guess_frame_id:=odom \
odom_frame_id:=icp_odom \
imu_topic:=/realsense_imu_publisher \
args:="-d  \
      --RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 0 \
      --Odom/ScanKeyFrameThr 0.8 \
      --OdomF2M/ScanMaxSize 999900000000  \
      --OdomF2M/ScanSubtractRadius 0.05   \
      --Icp/PM true \
      --Icp/VoxelSize 0.05   \
      --Icp/MaxTranslation 2   \
      --Icp/MaxCorrespondenceDistance 1.5 \
      --Icp/PMOutlierRatio 0.7 \
      --Icp/Iterations 150 \
      --Icp/PointToPlane true \
      --Icp/PMMatcherKnn 25 \
      --Icp/PMMatcherIntensity true \
      --Icp/PMMatcherEpsilon 1 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 25 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.1 \
      --RGBD/NeighborLinkRefining true \
      --RGBD/LinearUpdate 0.05"

And it is really weird because the icp_odometry node should connect them.

EDIT: I made a new capture, the previously odom named frame is now odom_guess, but odom (or icp_odom) and odom guess are still not connected but the rtab runs... should I publish a static transformation between them to be connected? Although, I noticed at this example that icp_odom and camera_pose_frame are not connected through a static transformation.

Also the odometry topic that is fed as guess to the odometry node is not coming from the imu but from the pose data taken by T265 using the pose  translation and rotation data.

I can not use the realsense2 because of kernel related issues so I am using the pyrealsense2 package in order to get the imu and odom topics, so the guess does not include imu at this stage, is this something I should consider, do you have any suggestion over this?

Bonus questions:
1) About the Icp/CorrespondenceRatio parameter, in what cases do you recommend to experiment with it ?
2) Also, if I set ScanKeyFrameThr to 0 what are the dangers to that ?

[Mikor]

Hi Mathieu,

I realized what you meant when you said that the odom topic includes the imu as it is a result of processing the imu data, but wouldn't the imu topic become obsolete when we use an odom guess which is a result of inertial sensor data plus video from the sensors of the T265? Or it is still a necessity to declare an imu topic so GTSAM can perform the graph optimization better by using the gravity constraints ?

My guess is that I'd still need an imu_topic as the icp is about point cloud registration and the use of a guess topic would not make it imu_topic obsolete because icp would just converge again at a wrong place and the gravity constraints could help in this case.. What is your opinion about that?

Kind regards,
Anthony.

[matlabbe]

Hi Anthony,

1) yes icp_odometry should have publish TF between odom_icp and odom frames, thus connecting the whole graph together. Using T265 odom pose, it should already include IMU data, so yes you may not need to feed imu to rtabmap at all. When we are receiving VIO poses, we can set Mem/UseOdomGravity to true to use VIO for gravity constraints in rtabmap (to help optimizing the graph based also on gravity). However, there is no guarentee that icp_odometry will follow the gravity, it is then recommended to feed imu topic to rtabmap too.

2) Icp/CorrespondenceRatio can be used to stop icp_odometry if there is not enough overlap between the current scan and the local map. For icp_odometry, is could be low (0.01 to 0.1) if you prefer a drift at some point instead of loosing tracking totally. For rtabmap node, it can be higher (0.1 to 0.4) to make sure loop closures and proximity detections have enough correspondences to accept them.

3) 0=Disabled. Odometry may drift more and even when not moving. A new keyframe is created every new scan. It depends on the environment and how much overlap there is between successive scans. In general I try to keep it lowest as possible to reduce odometry drift, but high enough to not lose tracking.

cheers,
Mathieu