Aruco Marker Landmarks error in Localisation after good mapping.

Been using RTABmap for ~ a year now... all good. Now tried the Aruco Marker Landmarks which seemed to work well for mapping, showing my three Landmarks recognised etc... Then with same params but in Localisation mode, I get these errors:

[ WARN] (2022-08-13 10:36:47.509) RegistrationIcp.cpp:552::computeTransformationImpl() ICP PointToPlane ignored as structural complexity cannot be computed (from=0.000000 to=0.000000)!? PointToPoint is done instead.
[ WARN] (2022-08-13 10:36:47.509) RegistrationIcp.cpp:687::computeTransformationImpl() libpointmatcher icp...temporary maxDist=0.05 (Icp/MaxCorrespondenceDistance=0.050000, Icp/VoxelSize=0.050000)
[ WARN] (2022-08-13 10:36:47.511) RegistrationIcp.cpp:816::computeTransformationImpl() not supposed to be here!
[ERROR] (2022-08-13 10:36:47.512) OptimizerG2O.cpp:890::optimize() Map: Failed adding constraint between -3 and -3, skipping
[ERROR] (2022-08-13 10:36:47.512) OptimizerG2O.cpp:890::optimize() Map: Failed adding constraint between -2 and -2, skipping
[ERROR] (2022-08-13 10:36:47.512) OptimizerG2O.cpp:890::optimize() Map: Failed adding constraint between -1 and -1, skipping

Setup is Turtlebot3 with Realsense D455, apt installed : ros-noetic-rtabmap-ros/focal,now 0.20.18-2focal.20220621.181925 amd64 [installed] params:

      <param name="RGBD/MarkerDetection"            type="string" value="true"/>      
      <param name="Marker/Dictionary"               type="string" value="10"/>
      <param name="Marker/Length"                   type="string" value="0.1705"/>
      <param name="Marker/MaxRange"                 type="string" value="2.5"/>            
      <param name="Marker/MinRange"                 type="string" value="0.35"/> 

I have a small .db example (88Mb) if it would be useful ? Any advice please. Thanks, Michael Hallett.

[matlabbe]

Hi,

The ICP warning is about failing to get appropriate normals, falling back to PointToPoint matching. There is maybe something wrong with the input scans. If you have a db, always useful to share it. Anyway, I think the transform was still computed.

For the landmark constraint errors, this is strange. I would need to test in localization mode to try to reproduce the error. Which package did you use for aruco marker detection?

cheers,
Mathieu

Thanks: now revisiting this with RTAB-Map version = 0.20.21

I use no "package for aruco marker detection" ( but markers are aruco_detect based).  I just set params in RTAB-Map and it now seems to work fine both in mapping and localisation.  Markers are detected well in rtabmapviz.

Still getting loop closure icp errors, (and ARUCO markers do not seem to help to remove these) e.g. in db downloadable from here at:  https://drive.google.com/file/d/1ljo05eKDJ8a7hxtJey4OZLxOEMgipqyk/view?usp=sharing 

typical erros =

[ INFO] [1668037684.773586831]: rtabmap (563): Rate=0.33s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.5541s, Maps update=0.0132s pub=0.0000s (local map=494, WM=491)
[ WARN] (2022-11-10 10:48:04.782) MainWindow.cpp:1923::processStats() Processing time (0.549191s) is over detection rate (0.333333s), real-time problem!
[ WARN] (2022-11-10 10:48:05.013) RegistrationIcp.cpp:552::computeTransformationImpl() ICP PointToPlane ignored as structural complexity cannot be computed (from=0.000000 to=0.000000)!? PointToPoint is done instead.
[ WARN] (2022-11-10 10:48:05.013) RegistrationIcp.cpp:687::computeTransformationImpl() libpointmatcher icp...temporary maxDist=0.05 (Icp/MaxCorrespondenceDistance=0.050000, Icp/VoxelSize=0.050000)
[ WARN] (2022-11-10 10:48:05.018) RegistrationIcp.cpp:816::computeTransformationImpl() not supposed to be here!
[ WARN] (2022-11-10 10:48:05.358) Rtabmap.cpp:3560::process() Rejecting all added loop closures (1, first is 564 <-> 541) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.016655 (edge 379->380, type=0, abs error=5.465732 deg, stddev=0.031623). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 2.500000 of std deviation.
[ WARN] (2022-11-10 10:48:05.358) Rtabmap.cpp:3585::process() Loop closure 564->541 rejected!
[ INFO] [1668037685.375852705]: rtabmap (564): Rate=0.33s, Limit=0.000s, Conversion=0.0011s, RTAB-Map=0.5886s, Maps update=0.0114s pub=0.0000s (local map=494, WM=491)

[matlabbe]

Hi,

The issue with ICP is that the scan format in the database is XYZ, instead of XY for 2d scans. Because of this, normals are computed wrongly assuming the scan is 3D. Normally, rtabmap_ros should generate XY format from sensor_msgs::LaserScan topics, then normals will be generated in 2D instead of 3D. If you are feeding a sensor_msgs::LaserScan topic, can you share the launch file you used for rtabmap node? There could be a bug here. However, if you are feding a sensor_msgs::PointCloud2, you would need to remove the Z channel before feeding it to rtabmap so that the scan is considered 2D.

cheers,
Mathieu

Thanks... re-testing this now... I removed the Z from the scan Pointcloud: RTABmap runs, but then does not seem to issue any odom>map tf: sample db attached @ https://drive.google.com/file/d/1YDEJucJCSaJCZHLpzS0QC4tmXp_E0FDN/view?usp=sharing

I convert my laser scan to a Pointcloud2 to correct for robot movement: e.g. discussed here http://www.buzzdev.guru/p/using-cartographer-ros-with-clockwise.html

Does RTABmap do this any way from a CLOCKWISE 5Hz laser scanner ? .. my tests using RTABmap and the raw laser-scan topic do not seem to be as good as compared to my corrected scan Pointcloud2.

Meanwhile, RVIZ does not seem to like this new 2d XY Pointcloud2 (e.g. without the Z)... but otherwise the topic message I am generating looks fine.  

My "normal" 3d Pointcloud2 from the scan generally works fine, with a constant Z value.  Can this not be detected by RTABmap to avoid the ICP 2d / 3d normal compute problem ?

OR: what is the expected RTABmap input format for "subscribe_scan_cloud" for XY 2d ?

[matlabbe]

Hi,

Hummm, I cannot confirm if laser_geometry handles clockwise scans correctly. Normally, here, the laser scan would be converted to point cloud and corrected depending on the odometry. I assume you are referring to rtabmap node (not icp_odometry).

The scan seems okay in the database. Only problem I see is that TF between the camera and base_link / laser link is wrong:


If you compute your point cloud outside, you can compute also the normals, and feed a XYZNormal point cloud to rtabmap.

[matlabbe]

For convenience, I updated rtabmap_ros in this commit with a new parameter called scan_cloud_is_2d for both icp_odometry and rtabmap nodes. If you use "subscribe_scan_cloud" with an input PointCloud2, when "scan_cloud_is_2d" is true, it will assume the input cloud is 2D (even if it has z channel). If normals have to be computed, they will then be correctly computed.

cheers,
Mathieu

I apologise... I made an error in my XY 2d scan-cloud.... (wrong frame-id)... corrected now and seems to work well.  Will test more, but so far ICP error also gone.

Thanks

Well that is also great = "new parameter called scan_cloud_is_2d" ... I will test this too.

What is your release cycle for apt installs: ros-noetic-rtabmap-ros/focal, amd64 ?

[matlabbe]

My schedule is not defined, it depends if significant updates have been made and I am not in a rush at work. It took me 3 hours making all the releases last weekend, more if something breaks on a specific distro.