How to improve mapping accuracy based on ArUco identification code?

[ChenTengyun]

Hi Mathieu,

    From the GitHub Wiki and rtabmap forum, I have seen a lot of your professional replies to rtabmap + marker questions. I judge you are an expert in this field and am glad to communicate with you. At present, I also have problems with rtabmap + marker. I hope to get your full support.

【Objectives I need to achieve and their basic ideas】:
1. Each mapping starts from the same place identified by an ArUco to ensure that the reference coordinate system is the same for each mapping;
2. ArUco identification codes with known 3D coordinates are deployed around the survey area in advance. Its reference coordinate system is the starting point of each mapping, and the IDs and coordinates of all ArUco identification codes in the area are configured to the rtabmap system in advance;
3. During each mapping, when the camera moves and detects the pre deployed ArUco identification code, the known coordinate value of the ArUco identification code is used to correct the error of the visual odometer in time;
4.Finally, the mapping accuracy of the whole area is improved.



【help context】:
1. Based on the existing rtabmap code on GitHub, the ArUco identification code is added to help improve the mapping accuracy. For this goal, a specific code modification scheme is proposed. I hope to hear your suggestions. Or provide some reference documents or reference codes.
2. Based on Ubuntu 18.04 + ROS melodic, modify the rtabmap code, compile the source code, replace the corresponding module of rtabmap in the system, run it and debug it. I hope you can provide some reference opinions and materials for this work.

cheers,
ChenTengyun

[matlabbe]

Hi ChenTengyun,

Currently rtabmap detects the tags and add their position to map based on current odometry. If a tag is seen again, like a loop closure detection, it will add constraints to close the loop and correct the map, while optimizing the position of that tag. What I can understand of what you are trying to do, is that knowing "a priori" exactly the 3D position of those tags, we would want to optimize the map while forcing them to keep their "a priori" position. This could be possible to do by adding a Prior constraint on the pose of the tag (xyz and/or orientation) to a fixed value before optimizing the graph. To do so, rtabmap would have to load a config file telling that tag 1 is at some position, tag 2 at this other position and so on. The prior of the tag could be added just after this line, where jter->first is the id of the tag, then do:

// Get MARKER_ABSOLUTE_POSE from the config file
links.insert(std::make_pair(jter->first, Link(jter->first,jter->first, kPosePrior, MARKER_ABSOLUTE_POSE, cv::Mat::eye(6,6,CV_64FC1)*0.001)));

That prior will be eventually added to g2o optimization here (when g2o is used).

cheers,
Mathieu

[javad0111]

Hi Mathieu,

I am working on the same problem as ChenTengyun. But I want to try the case when one Tag with special id would be the origin of Map frame (or maybe map frame), that's why I added an if-condition to have prior constraint for only one tag. Changes are added here as following:

if(!uContains(poses, jter->first))
{
        poses.insert(std::make_pair(jter->first, poses.at(*iter) * jter->second.transform()));
}
links.insert(std::make_pair(jter->first, jter->second.inverse()));
Link::Type kPosePrior;
if (jter->first== 104){
        Transform MARKER_ABSOLUTE_POSE=Transform(0, 0, 0, 0, 0, 0, 1);
        std::cout<<"check inside the code of uContains ******************"<<std::endl; //for checking if this line is read.
        links.insert(std::make_pair(jter->first, Link(jter->first,jter->first, kPosePrior, MARKER_ABSOLUTE_POSE, cv::Mat::eye(6,6,CV_64FC1)*0.001)));
}

But the problem is I see no changes with this modification! I would appreciate it if you could help me out, thanks in advance.

Kind regards,
Javad

[matlabbe]

Hi Javad,

In your code, the landmark should have negative ids.

Note that a marker prior parameter has been added recently to do so.

rtabmap --params | grep Marker/Priors
Param: Marker/Priors = ""                                  [World prior locations of the markers. The map will be transformed in marker's
                                                            world frame when a tag is detected. Format is the marker's ID followed by its position 
                                                            (angles in rad), markers are separated by vertical line ("id1 x y z roll pitch yaw|id2 x y z
                                                             roll pitch yaw"). Example:  "1 0 0 1 0 0 0|2 1 0 1 0 0 1.57" (marker 2 is 1 meter forward
                                                             than marker 1 with 90 deg yaw rotation).]
Param: Marker/PriorsVarianceAngular = "0.001"              [Angular variance to set on marker priors.]
Param: Marker/PriorsVarianceLinear = "0.001"               [Linear variance to set on marker priors.]

In your case, setting this:

Marker/Priors="104 10 0 0 0 0 0"

will make the map translate so that landmark 104 is on absolute pose 10 meters in x.

[javad0111]

Thanks for the reply.

1) I see that Marker/Prior parameter was recently added. Does it mean that I do not need to change the source code? So removing the part that I mentioned in my previous message.

2) I did the steps below, but I still see no changes or  translation from map frame: here is the picture of built map and frames.



     i. launching this launch file: aruco_multiple.launch
     ii. launching this launch file: rtabmao_l515_bag.launch
     iii. playing the bag file that publishes depth image , rbd image, camera info, and tf_static topics. There are three tags in the images with id of 104, 100, and 101. Link to the bag file: l515_with_aruco_42_seconds.bag
     iv. rosparam set /rtabmap/rtabmap/Marker/Priors "104 0 0 0 0 0 0"

Thanks again and kind regards,
Javad

[matlabbe]

1) Yes

2) Forgot to mention that you need to set Optimizer/PriorsIgnored to false too, while also calling update_parameters service so that rtabmap actually read again the modified rosparam:

roslaunch rtabmap_l515_bag.launch use_sim_time:=true
roslaunch aruco_multiple.launch
rosparam set /rtabmap/rtabmap/Marker/Priors "104 0 0 0 0 0 0" 
rosparam set /rtabmap/rtabmap/Optimizer/PriorsIgnored false
rosservice call /rtabmap/update_parameters 
rosbag play --clock --pause l515_with_aruco_42_seconds.bag

cheers,
Mathieu

[xsasdA]

matlabbe wrote

Hi Javad,

In your code, the landmark should have negative ids.

Note that a marker prior parameter has been added recently to do so.

rtabmap --params | grep Marker/Priors
Param: Marker/Priors = ""                                  [World prior locations of the markers. The map will be transformed in marker's
                                                            world frame when a tag is detected. Format is the marker's ID followed by its position 
                                                            (angles in rad), markers are separated by vertical line ("id1 x y z roll pitch yaw|id2 x y z
                                                             roll pitch yaw"). Example:  "1 0 0 1 0 0 0|2 1 0 1 0 0 1.57" (marker 2 is 1 meter forward
                                                             than marker 1 with 90 deg yaw rotation).]
Param: Marker/PriorsVarianceAngular = "0.001"              [Angular variance to set on marker priors.]
Param: Marker/PriorsVarianceLinear = "0.001"               [Linear variance to set on marker priors.]

In your case, setting this:

Marker/Priors="104 10 0 0 0 0 0"

will make the map translate so that landmark 104 is on absolute pose 10 meters in x.

Hello Matlabbe,

I have a question to this param in combination with the following issue on GH: https://github.com/introlab/rtabmap/issues/712

I have a similar setup like the user schmiran with the exception that I don't have a world frame set up. In my case it's just the map frame that is present and similar frames/sensors for the rest of the tf tree.

Mapping and Apriltag detection is working fine, example tags get recognized. The map frame is currently located at the odom_frame (i think this is the default), but I could just create a new "world" frame with tf in order to have the same starting point as the user schmiran.
The apriltags are spread out around the room and each of them has a known location in global coordinates which I saved in an XML file.

The robot will start at a random location with local coordinates and as soon as it detects one of the markers, the transformation of the local coordinates to the detected images should be available via topic tag_detections with apriltag_ros. This way, I get camera_link -> tag. Now, I need to get the transform from local to global coordinates in order to tell rtabmap to output the pointclouds generated in this coordinate system.

TLDR: I want to achieve the same goal of transforming between map/local coordinates and world/model/global coordinates with the help of apriltags that have known world/model/global coordinates in order to transform the point clouds that RTABMAP creates in the global/world coordinate system.

I would use the map frame and transform it to the location of the world coordinate origin which should be similar to what schmiran was trying to do. Does this param here do the job of actually achieving my goal of instantly finding this transform and applying it? I'm using the external apriltag_ros package, is it even possible to use that parameter which is normally only available for the build-in apriltag/aruco detection?

I already searched for this parameter and it appears to not be implemented in 0.19.x. Can i manually add the functionality of this parameter in the source code without changing anything else, or is it needed to update Rtabmap? Which version does support these parameters?

Is there a better way to get this transformation done?
Maybe I could add an Apriltag directly at the world coordinate origin which needs to be detected everytime? This ofcourse will limit the robot in its path because it has to visit that location first everytime, but it would be much simpler because map -> base_link -> camera_frame -> tag = world_frame.

Have a great day and sorry for reviving this rather old thread!

[matlabbe]

Hi,

Parameter `/rtabmap/rtabmap/Marker/Priors` seems to be what you want. The know position of the tags in your XML file could be converted in that parameter. Rtabmap should always received relative position of the tag (camera frame -> tag frame), but with knowing the priors of the tag, the map's graph would automatically transform in world tag frame. Ideally detecting more than one tag will help to better estimate the orientation of the map to world frame.

To have Marker/Priors, you need latest rtabmap, or at least 0.20.21: https://github.com/introlab/rtabmap/commit/b646c5e1db77a685aa400696107dc0f9628eb914 (note that ros binaries are at 0.20.21).

cheers,
Mathieu

[xsasdA]

Hi Mathieu,

thank you very much for the information regarding the Marker/Priors parameter.

One question to this:

Ideally detecting more than one tag will help to better estimate the orientation of the map to world frame.

 Is it necessary that several markers are detected at the same time or is it sufficient if new markers are detected from time to time?
Do I need to set any other parameter for the better estimation of the orientation from map -> world with multiple detected markers to happen?

I will try to update rtabmap to the least required version, thanks for the information!

Best regards,
xsasda

[xsasdA]

One more question:

When building rtabmap with 'tags/0.20.21' from source, there is no corresponding 'rtabmap_ros' tag with version '0.20.21' to build (only 0.20.20/22 are available). If I try to use version 0.20.22 or 0.20.20 I get the following error that not the right config file can be found:

The following configuration files were considered but not accepted:

/usr/local/lib/rtabmap-0.19/RTABMapConfig.cmake, version: 0.19.3
/usr/local/lib/rtabmap-0.20/RTABMapConfig.cmake, version: 0.20.21

An easy fix would be to use the 20.22 version ofcourse, but I was just wondering why this tag is missing in rtabmap_ros but is available in rtabmap.

[matlabbe]

I was updating the tags yesterday, 0.20.22 can be found in both rtabmap and rtabmap_ros projects. You should rebuild/install rtabmap library.

And yes, when I say detecting more than one tag, it doesn't mean at the same time, just two tags in the environment.

[xsasdA]

Hello,
thank you very much for the information.

I was able to update RTABMap now. In order to make it work, I had to update librealsense packages as well. Otherwise, I got this error when trying to install rtabmap standalone libraries "rs2::sensor’ has no member named ‘get_active_streams’; did you mean ‘get_option_name’?'" in CameraStereoImages2.cpp.

(Because we were talking about upgrading rtabmap in order to be able to use Marker/Priors, I decided to post my questions/problems regarding this upgrade here. If this is not the right place to post it here, I can repost it as a new question on rtabmap forum or/and realsense forum. Feel free to tell me and sorry in advance.)

If I try to replay my old bagfile now, I get the following errors:

In the rosbag play terminal:

[ERROR] [1670926021.493159740, 1616584700.766188181]: Client [/rtabmap/rtabmap] wants topic /d435_1/rgbd_image to have datatype/md5sum [rtabmap_ros/RGBDImage/affef6cc8804ffba98ce6ed6f1ca8942], but our version has [rtabmap_ros/RGBDImage/a7fa8f34f44b976b3b2dec13c02f9f0e]. Dropping connection.
[ERROR] [1670926021.494318351, 1616584700.766188181]: Client [/rtabmap/rtabmap] wants topic /d435_2/rgbd_image to have datatype/md5sum [rtabmap_ros/RGBDImage/affef6cc8804ffba98ce6ed6f1ca8942], but our version has [rtabmap_ros/RGBDImage/a7fa8f34f44b976b3b2dec13c02f9f0e]. Dropping connection.

In the launchfile terminal that is running rtabmap, apriltag_ros and rviz nodes:

[ WARN] [1670926021.498743601, 1616584700.766188181]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /d435_1/rgbd_image \
   /d435_2/rgbd_image \
   /velodyne_points

I checked all the topics, and they do get published (velodyne_points with 5Hz and d435_1&2/rgbd_image with 30Hz):
d435_1 & 2:

[...]
average rate: 29.597
	min: 0.030s max: 0.043s std dev: 0.00440s window: 27
average rate: 29.731
	min: 0.020s max: 0.043s std dev: 0.00495s window: 56
average rate: 29.674
	min: 0.020s max: 0.043s std dev: 0.00516s window: 86
[...]

velodyne_points:

[...]
average rate: 5.137
	min: 0.163s max: 0.207s std dev: 0.01302s window: 9
average rate: 5.078
	min: 0.163s max: 0.207s std dev: 0.01097s window: 14
average rate: 5.054
	min: 0.163s max: 0.207s std dev: 0.00975s window: 19
[...]

I also tried to fix the bag file by checking it first:

rosbag check ivi-apriltag-2.bag
The following migrations need to occur:
 * From: realsense2_camera/IMUInfo [a02adb3a99530b11ba18a16f40f9512a]
   To:   Unknown
    1 rules missing:
     * From: realsense2_camera/IMUInfo [a02adb3a99530b11ba18a16f40f9512a]
       To:   Unknown
 * From: rtabmap_ros/RGBDImage [a7fa8f34f44b976b3b2dec13c02f9f0e]
   To:   rtabmap_ros/RGBDImage [affef6cc8804ffba98ce6ed6f1ca8942]
    1 rules missing:
     * From: rtabmap_ros/RGBDImage [a7fa8f34f44b976b3b2dec13c02f9f0e]
       To:   rtabmap_ros/RGBDImage [affef6cc8804ffba98ce6ed6f1ca8942]
 * From: realsense2_camera/Extrinsics [3627b43073f4cd5dd6dc179a49eda2ad]
   To:   Unknown
    1 rules missing:
     * From: realsense2_camera/Extrinsics [3627b43073f4cd5dd6dc179a49eda2ad]
       To:   Unknown

This seems to indicate that the topics and msg-types have changed.
Do I need to rerecord a bagfile with the new versions to match the new hash of the rtabmap_ros/RGBDImage Topic and would this fix the realsense topic mismatches too? I'm not sure how to fix the migrations that have an Unknown target.

Best regards,
xsasda

[matlabbe]

xsasdA wrote

I was able to update RTABMap now. In order to make it work, I had to update librealsense packages as well. Otherwise, I got this error when trying to install rtabmap standalone libraries "rs2::sensor’ has no member named ‘get_active_streams’; did you mean ‘get_option_name’?'" in CameraStereoImages2.cpp.

Should I understand with latest realsense version it builds correctly? If so, then it is expected (I triy to be compatible with latest version that people will usually use). Which realsense version were you using? I could see if I should do a backward compatibility patch for old realsense libraries if they are still the current binaries.

[ERROR] [1670926021.493159740, 1616584700.766188181]: Client [/rtabmap/rtabmap] wants topic /d435_1/rgbd_image to have datatype/md5sum [rtabmap_ros/RGBDImage/affef6cc8804ffba98ce6ed6f1ca8942], but our version has [rtabmap_ros/RGBDImage/a7fa8f34f44b976b3b2dec13c02f9f0e]. Dropping connection.
[ERROR] [1670926021.494318351, 1616584700.766188181]: Client [/rtabmap/rtabmap] wants topic /d435_2/rgbd_image to have datatype/md5sum [rtabmap_ros/RGBDImage/affef6cc8804ffba98ce6ed6f1ca8942], but our version has [rtabmap_ros/RGBDImage/a7fa8f34f44b976b3b2dec13c02f9f0e]. Dropping connection.

Humm, the RGBDImage msg has been updated and doesn't match the one in your bag. Do you care about this bag? or creating a new one is not too much trouble. Otherwise, you may have to downgrade RGBDImage msg to a previous version. See history here, though corresponding code would need to be downgrade. Ideally, when you record rosbags, record the standard msgs if possible.

cheers,
Mathieu

[xsasdA]

Hello Mathieu,

thank you for the fast response!

Should I understand with latest Realsense version it builds correctly? If so, then it is expected (I triy to be compatible with latest version that people will usually use). Which Realsense version were you using? I could see if I should do a backward compatibility patch for old Realsense libraries if they are still the current binaries.

Yes, you are right that everything is building fine with the latest Realsense version. I was using version 2.33.1 before which is around 2.75 years old. It was honestly my fault, I just forgot to update the SDK. It's probably better to continue working with the new version, if it doesn't break anything in my current setup and it's enough to record a new Bagfile.

Humm, the RGBDImage msg has been updated and doesn't match the one in your bag. Do you care about this bag? or creating a new one is not too much trouble.

I'll be able to record another Bagfile next week when I'm back in the test laboratory, so the existing Bagfile wouldn't be too important.
Do you think that all the issues with RTABMap and the missing topic publishers get fixed by recording a new Bagfile?

Otherwise, you may have to downgrade RGBDImage msg to a previous version. See history here, though corresponding code would need to be downgrade.

Thanks for the information, I will try using this alternative if I can't record a bagfile next week.

 Ideally, when you record rosbags, record the standard msgs if possible.

Thanks for the hint, I will remember to include standard messages.

Best regards,
xsasda

[matlabbe]

xsasdA wrote

Do you think that all the issues with RTABMap and the missing topic publishers get fixed by recording a new Bagfile?

For the issues above, yes.

Note that I added a new node called rgbd_split in that commit, it can split RGBDImage into standard messages. If you really want to recover your old bag, you may port that code into an older rtabmap version, then replay bag with that node to split the messages into standard ones, recording then a new bag from the previous one. Then update rtabmap and re-use that new bag.

[xsasdA]

Hello Mathieu,

For the issues above, yes.

thank you - good to know.

Note that I added a new node called rgbd_split in that commit, it can split RGBDImage into standard messages. If you really want to recover your old bag, you may port that code into an older rtabmap version, then replay bag with that node to split the messages into standard ones, recording then a new bag from the previous one. Then update rtabmap and re-use that new bag.

Wow, that is awesome Mathieu! Thank you so much for your commitment. I will try this as soon as possible.

Best regards,
xsasda

[xsasdA]

Hello Mathieu,

I have some updates and follow-up-questions. I was able to get a bag file working now and tried to apply the Marker/Priors parameter like so:

1 2 3

<param name="Optimizer/PriorsIgnored" type="bool" value="false" /> <param name="Marker/Priors" type="string" value="1 -3.56 1.583 0.50 0 1.5708 0|2 -1.495 1.605 4.91 0 3.14159 0" />

I was able to obtain a good map with loop closure. Unfortunately, the map frame didnt get transformed correctly, although marker/priors was set and markers got detected.

This is the actual output in rviz after running rtabmap node:

This is the desired output, with using parameter Marker/Priors to transform the map frame:

(the painted coordinate system with distance 0.45 to the corner of the room is the desired coordinate origin, with x facing downwards, z left and y up and the real output of the map frame is measured to be around 1.2m)

I also looked up the generated pointcloud and the graphview, if the transformation was later applied to it, but unfortunately not: The coordinate system is still the same.

Do you know why the map_frame/pointcloud does not get transformed correctly? What do I need to change?

Attachments:

node output of rtabmap: roslaunch_output.txt (I usually can ignore the tf warning, after restarting the bag file it is most likely gone.)
roslaunch file: roslaunch_file.txt

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95

<launch> <arg name="rviz" default="true" /> <param name="/use_sim_time" value="true" /> <node pkg="tf" type="static_transform_publisher" name="base_to_d435_1_tf" args="0.375 -0.35 0.825 -0.52 0.0 3.14 /base_link /d435_1_link 200" /> <node pkg="tf" type="static_transform_publisher" name="base_to_d435_2_tf" args="0.375 0.35 0.825 0.52 0.0 0.0 /base_link /d435_2_link 200" /> <node pkg="tf" type="static_transform_publisher" name="base_to_t265_tf" args="0.36 0.0 0.95 0.0 0.0 0.0 /base_link /t265_link 200" /> <node pkg="tf" type="static_transform_publisher" name="base_to_d415_tf" args="0.36 0.02 0.92 0.0 0.0 0.0 /base_link /d415_link 200" /> <node pkg="tf" type="static_transform_publisher" name="base_to_vlp16_tf" args="0.0 0.0 1.254 1.57 0.0 0.0 /base_link /velodyne 200" /> <group ns="rtabmap"> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start"> <remap from="rgbd_image0" to="/d435_1/rgbd_image" /> <remap from="rgbd_image1" to="/d435_2/rgbd_image" /> <remap from="odom" to="/t265/odom/sample" /> <remap from="scan_cloud" to="/velodyne_points" /> <param name="subscribe_rgb" type="bool" value="false" /> <param name="subscribe_depth" type="bool" value="false" /> <param name="subscribe_rgbd" type="bool" value="true" /> <param name="subscribe_scan_cloud" type="bool" value="true" /> <param name="odom_topic" value="/t265/odom/sample" /> <param name="scan_cloud_topic" value="/velodyne_points" /> <param name="odom_frame_id" type="string" value="t265_odom_frame" /> <param name="odom_tf_angular_variance" type="double" value="0.0005" /> <param name="odom_tf_linear_variance" type="double" value="0.0001" /> <param name="frame_id" type="string" value="base_link" /> <param name="rgbd_cameras" type="int" value="2" /> <param name="Grid/FromDepth" type="string" value="false" /> <param name="Vis/EstimationType" type="string" value="0" /> <param name="approx_sync" type="bool" value="true" /> <param name="cloud_noise_filtering_radius" value="0.05" /> <param name="cloud_noise_filtering_min_neighbors" value="2" /> <param name="Reg/Force3DoF" value="true" /> <param name="RGBD/NeighborLinkRefining" value="true" /> <param name="Reg/Strategy" value="1" /> <param name="Kp/MaxFeatures" type="string" value="10000" /> <param name="RGBD/LoopClosureReextractFeatures" type="bool" value="true" /> <param name="Optimizer/Strategy" type="string" value="1" /> <param name="Optimizer/PriorsIgnored" type="bool" value="false" /> <param name="Marker/Priors" type="string" value="1 -3.56 1.583 0.50 0 1.5708 0|2 -1.495 1.605 4.91 0 3.14159 0" /> <param name="Icp/VoxelSize" type="string" value="0.5" /> <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1" /> </node> </group> <param name="robot_description" textfile="platform.urdf" /> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" output="screen" args="-d mapping_2d_edited.rviz" /> <arg name="launch_prefix" default="" /> <arg name="node_namespace" default="apriltag_ros_continuous_node" /> <arg name="camera_name" default="/d415" /> <arg name="camera_frame" default="d415_link" /> <arg name="image_topic" default="/color/image_raw" /> <rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="$(arg node_namespace)" /> <rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="$(arg node_namespace)" /> <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)"> <remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" /> <remap from="camera_info" to="$(arg camera_name)/camera_info" /> <param name="camera_frame" type="str" value="$(arg camera_frame)" /> <param name="publish_tag_detections_image" type="bool" value="true" /> </node> </launch>

rtabmap database file: RTABMap DB File (MS OneDrive)
bag file: (Its not optimal, stopped to early on tag2, thats why it is only detected while moving and therefore introduces some error.)
Bag File (MS OneDrive) (If you are prompted to login, just navigate back to the previous site or press ALT+LEFT ARROW and it should work.)

rviz output after playing rosbag:

tf_tree:

What is also strange is that when I want to query transformations between "base_link" and "tag1" with "tf_echo" or "tf_monitor", this does not work. tf_echo outputs

Could not find a connection between 'tag1' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.

and tf_monitor waits forever for this tf. But this should work, because "base_link" -> "d415_link" -> ... -> "d415_color_optical_frame" -> "tag1", as you can see in the picture.

Best regards
xsasda

[matlabbe]

Hi,

The tags are not added to map (you should see green links in the occupancy grid view), probably because of your TF error. Can you see the tag frame in RVIZ when they are detected?

Check also:

rosrun tf tf_echo base_link tag1

[xsasdA]

Hey

rosrun tf tf_echo base_link tag1

 sometimes returns

Failure at 1671607361.331520417
Exception thrown:Could not find a connection between 'base_link' and 'tag1' because they are not part of the same tree.Tf has two or more unconnected trees.
The current list of frames is:
Frame d435_1_link exists with parent base_link.
Frame base_link exists with parent t265_pose_frame.
Frame d435_2_link exists with parent base_link.
Frame t265_pose_frame exists with parent t265_odom_frame.
Frame t265_odom_frame exists with parent map.
Frame t265_link exists with parent base_link.
Frame d415_link exists with parent base_link.
Frame velodyne exists with parent base_link.
Frame d435_2_depth_frame exists with parent d435_2_link.
Frame d435_2_depth_optical_frame exists with parent d435_2_depth_frame.
Frame d435_2_color_frame exists with parent d435_2_link.
Frame d435_2_color_optical_frame exists with parent d435_2_aligned_depth_to_color_frame.
Frame d435_2_aligned_depth_to_color_frame exists with parent d435_2_link.
Frame tag1 exists with parent d415_color_optical_frame.
Frame tag2 exists with parent d415_color_optical_frame.

and if I restart the bag and launchfile a couple of times it suddenly works:

At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]

Rviz is also clearly showing the tag frame all the time, while tf_echo only shows it sometimes.


I dont know why this changes everytime and is inconsistent.

This is the output when the tag frame is showing in rviz and also gets published when running tf_echo:

[ INFO] [1672085149.915733152]: rtabmap 0.20.22 started...
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
QObject::connect: Cannot queue arguments of type 'QVector<int>'
(Make sure 'QVector<int>' is registered using qRegisterMetaType().)
[ WARN] [1672085155.918346265, 1671607327.706539269]: Graph has changed! The whole cloud is regenerated.
[ INFO] [1672085155.918995641, 1671607327.706539269]: Assembled 1 obstacle and 1 ground clouds (3084 points, 0.000666s)
[ INFO] [1672085155.919231154, 1671607327.706539269]: rtabmap (1): Rate=1.00s, Limit=0.000s, Conversion=0.3435s, RTAB-Map=0.0562s, Maps update=0.0019s pub=0.0009s (local map=1, WM=1)
[ INFO] [1672085156.682730310, 1671607328.472649162]: Assembled 0 obstacle and 0 ground clouds (3084 points, 0.000007s)
[ INFO] [1672085156.682926731, 1671607328.472649162]: rtabmap (2): Rate=1.00s, Limit=0.000s, Conversion=0.0021s, RTAB-Map=0.1274s, Maps update=0.0033s pub=0.0002s (local map=1, WM=1)
[ INFO] [1672085157.781890032, 1671607329.573166571]: Assembled 0 obstacle and 0 ground clouds (3084 points, 0.000005s)
[ INFO] [1672085157.782136749, 1671607329.573166571]: rtabmap (3): Rate=1.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.2027s, Maps update=0.0054s pub=0.0002s (local map=1, WM=1)
[ INFO] [1672085158.566854579, 1671607330.350103613]: Assembled 1 obstacle and 1 ground clouds (4547 points, 0.001099s)
[ INFO] [1672085158.567038972, 1671607330.360179691]: rtabmap (4): Rate=1.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.0576s, Maps update=0.0015s pub=0.0013s (local map=2, WM=2)
[ INFO] [1672085159.762595321, 1671607331.544776444]: Assembled 1 obstacle and 1 ground clouds (6381 points, 0.002939s)
[ INFO] [1672085159.763106752, 1671607331.544776444]: rtabmap (5): Rate=1.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.2335s, Maps update=0.0042s pub=0.0035s (local map=3, WM=3)
[ INFO] [1672085160.717351405, 1671607332.505918512]: Assembled 1 obstacle and 1 ground clouds (8437 points, 0.002401s)
[ INFO] [1672085160.717719630, 1671607332.505918512]: rtabmap (6): Rate=1.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.2208s, Maps update=0.0021s pub=0.0028s (local map=4, WM=4)
[ INFO] [1672085161.782531396, 1671607333.569332964]: Assembled 1 obstacle and 1 ground clouds (10164 points, 0.004941s)
[ INFO] [1672085161.783106014, 1671607333.569332964]: rtabmap (7): Rate=1.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.2464s, Maps update=0.0045s pub=0.0055s (local map=5, WM=5)
[ INFO] [1672085162.746731186, 1671607334.535596350]: Assembled 1 obstacle and 1 ground clouds (11704 points, 0.005113s)
[ INFO] [1672085162.747487433, 1671607334.535596350]: rtabmap (8): Rate=1.00s, Limit=0.000s, Conversion=0.0039s, RTAB-Map=0.2250s, Maps update=0.0045s pub=0.0059s (local map=6, WM=6)
[ INFO] [1672085163.740559756, 1671607335.529269701]: Assembled 1 obstacle and 1 ground clouds (13212 points, 0.003867s)
[ INFO] [1672085163.741133477, 1671607335.529269701]: rtabmap (9): Rate=1.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.2017s, Maps update=0.0040s pub=0.0045s (local map=7, WM=7)
[ INFO] [1672085164.777361099, 1671607336.566109024]: Assembled 1 obstacle and 1 ground clouds (14800 points, 0.006389s)
[ INFO] [1672085164.778358691, 1671607336.566109024]: rtabmap (10): Rate=1.00s, Limit=0.000s, Conversion=0.0032s, RTAB-Map=0.2669s, Maps update=0.0048s pub=0.0074s (local map=8, WM=8)
[ INFO] [1672085165.728770096, 1671607337.519526442]: Assembled 1 obstacle and 1 ground clouds (16644 points, 0.006565s)
[ INFO] [1672085165.729634144, 1671607337.519526442]: rtabmap (11): Rate=1.00s, Limit=0.000s, Conversion=0.0019s, RTAB-Map=0.1876s, Maps update=0.0053s pub=0.0074s (local map=9, WM=9)
[ INFO] [1672085166.849968461, 1671607338.633045496]: Assembled 1 obstacle and 1 ground clouds (18383 points, 0.009256s)
[ INFO] [1672085166.850963886, 1671607338.643147602]: rtabmap (12): Rate=1.00s, Limit=0.000s, Conversion=0.0039s, RTAB-Map=0.2896s, Maps update=0.0050s pub=0.0103s (local map=10, WM=10)
[ INFO] [1672085167.747671039, 1671607339.531025854]: Assembled 1 obstacle and 1 ground clouds (20557 points, 0.001885s)
[ INFO] [1672085167.748100912, 1671607339.531025854]: rtabmap (13): Rate=1.00s, Limit=0.000s, Conversion=0.0040s, RTAB-Map=0.2178s, Maps update=0.0016s pub=0.0023s (local map=11, WM=11)
[ INFO] [1672085168.928839185, 1671607340.716331257]: Assembled 1 obstacle and 1 ground clouds (22825 points, 0.010493s)
[ INFO] [1672085168.930344351, 1671607340.716331257]: rtabmap (14): Rate=1.00s, Limit=0.000s, Conversion=0.0040s, RTAB-Map=0.3445s, Maps update=0.0060s pub=0.0120s (local map=12, WM=12)
[ INFO] [1672085169.945472545, 1671607341.732082369]: Assembled 1 obstacle and 1 ground clouds (25009 points, 0.009224s)
[ INFO] [1672085169.946553382, 1671607341.732082369]: rtabmap (15): Rate=1.00s, Limit=0.000s, Conversion=0.0036s, RTAB-Map=0.3504s, Maps update=0.0035s pub=0.0103s (local map=13, WM=13)
[ INFO] [1672085170.907639929, 1671607342.680193129]: Assembled 1 obstacle and 1 ground clouds (26901 points, 0.011770s)
[ INFO] [1672085170.909249749, 1671607342.700296234]: rtabmap (16): Rate=1.00s, Limit=0.000s, Conversion=0.0035s, RTAB-Map=0.3531s, Maps update=0.0042s pub=0.0134s (local map=14, WM=14)
[ INFO] [1672085171.905237436, 1671607343.692873002]: Assembled 1 obstacle and 1 ground clouds (28440 points, 0.010649s)
[ INFO] [1672085171.906615959, 1671607343.692873002]: rtabmap (17): Rate=1.00s, Limit=0.000s, Conversion=0.0035s, RTAB-Map=0.3443s, Maps update=0.0049s pub=0.0120s (local map=15, WM=15)
[ INFO] [1672085172.677761718, 1671607344.470457338]: Assembled 1 obstacle and 1 ground clouds (30004 points, 0.005349s)
[ INFO] [1672085172.678650664, 1671607344.470457338]: rtabmap (18): Rate=1.00s, Limit=0.000s, Conversion=0.0016s, RTAB-Map=0.1479s, Maps update=0.0026s pub=0.0063s (local map=16, WM=16)
[ INFO] [1672085173.902305069, 1671607345.669512464]: Assembled 1 obstacle and 1 ground clouds (31921 points, 0.012458s)
[ INFO] [1672085173.903967593, 1671607345.669512464]: rtabmap (19): Rate=1.00s, Limit=0.000s, Conversion=0.0033s, RTAB-Map=0.3566s, Maps update=0.0047s pub=0.0141s (local map=17, WM=17)
[ INFO] [1672085174.661914349, 1671607346.451419850]: Assembled 1 obstacle and 1 ground clouds (33787 points, 0.006420s)
[ INFO] [1672085174.662751492, 1671607346.451419850]: rtabmap (20): Rate=1.00s, Limit=0.000s, Conversion=0.0015s, RTAB-Map=0.1309s, Maps update=0.0021s pub=0.0073s (local map=18, WM=18)
[ INFO] [1672085175.900535605, 1671607347.685600594]: Assembled 1 obstacle and 1 ground clouds (35402 points, 0.006477s)
[ INFO] [1672085175.901310365, 1671607347.685600594]: rtabmap (21): Rate=1.00s, Limit=0.000s, Conversion=0.0042s, RTAB-Map=0.2978s, Maps update=0.0018s pub=0.0073s (local map=19, WM=19)
[ INFO] [1672085176.813359378, 1671607348.605718081]: Assembled 1 obstacle and 1 ground clouds (36559 points, 0.009783s)
[ INFO] [1672085176.814727566, 1671607348.605718081]: rtabmap (22): Rate=1.00s, Limit=0.000s, Conversion=0.0027s, RTAB-Map=0.2277s, Maps update=0.0050s pub=0.0112s (local map=20, WM=20)
[ INFO] [1672085177.778980863, 1671607349.557092200]: Assembled 0 obstacle and 0 ground clouds (36559 points, 0.000021s)
[ INFO] [1672085177.779186553, 1671607349.557092200]: rtabmap (23): Rate=1.00s, Limit=0.000s, Conversion=0.0047s, RTAB-Map=0.1986s, Maps update=0.0037s pub=0.0002s (local map=20, WM=20)
[ INFO] [1672085178.740078514, 1671607350.527600147]: Assembled 0 obstacle and 0 ground clouds (36559 points, 0.000020s)
[ INFO] [1672085178.740281380, 1671607350.527600147]: rtabmap (24): Rate=1.00s, Limit=0.000s, Conversion=0.0024s, RTAB-Map=0.1725s, Maps update=0.0031s pub=0.0002s (local map=20, WM=20)
[ INFO] [1672085179.887411092, 1671607351.668895622]: Assembled 1 obstacle and 1 ground clouds (37569 points, 0.015533s)
[ INFO] [1672085179.889259107, 1671607351.668895622]: rtabmap (25): Rate=1.00s, Limit=0.000s, Conversion=0.0020s, RTAB-Map=0.3014s, Maps update=0.0069s pub=0.0174s (local map=21, WM=21)
[ INFO] [1672085180.961439926, 1671607352.747772848]: Assembled 1 obstacle and 1 ground clouds (38621 points, 0.015954s)
[ INFO] [1672085180.963240013, 1671607352.747772848]: rtabmap (26): Rate=1.00s, Limit=0.000s, Conversion=0.0045s, RTAB-Map=0.3449s, Maps update=0.0059s pub=0.0178s (local map=22, WM=22)
[ INFO] [1672085181.789100661, 1671607353.576539891]: Assembled 1 obstacle and 1 ground clouds (40156 points, 0.006776s)
[ INFO] [1672085181.789920538, 1671607353.576539891]: rtabmap (27): Rate=1.00s, Limit=0.000s, Conversion=0.0040s, RTAB-Map=0.2106s, Maps update=0.0019s pub=0.0076s (local map=23, WM=23)
[ INFO] [1672085182.880709688, 1671607354.671541401]: Assembled 1 obstacle and 1 ground clouds (41945 points, 0.005060s)
[ INFO] [1672085182.881518759, 1671607354.671541401]: rtabmap (28): Rate=1.00s, Limit=0.000s, Conversion=0.0040s, RTAB-Map=0.3036s, Maps update=0.0013s pub=0.0059s (local map=24, WM=24)
[ INFO] [1672085183.765075529, 1671607355.558007860]: Assembled 1 obstacle and 1 ground clouds (44317 points, 0.013590s)
[ INFO] [1672085183.766321467, 1671607355.558007860]: rtabmap (29): Rate=1.00s, Limit=0.000s, Conversion=0.0021s, RTAB-Map=0.1839s, Maps update=0.0031s pub=0.0149s (local map=25, WM=25)
[ INFO] [1672085184.781322286, 1671607356.570946796]: Assembled 1 obstacle and 1 ground clouds (46123 points, 0.015989s)
[ INFO] [1672085184.783252283, 1671607356.570946796]: rtabmap (30): Rate=1.00s, Limit=0.000s, Conversion=0.0019s, RTAB-Map=0.1932s, Maps update=0.0038s pub=0.0179s (local map=26, WM=26)
[ INFO] [1672085185.823010572, 1671607357.615244846]: Assembled 1 obstacle and 1 ground clouds (47776 points, 0.015172s)
[ INFO] [1672085185.824776329, 1671607357.615244846]: rtabmap (31): Rate=1.00s, Limit=0.000s, Conversion=0.0027s, RTAB-Map=0.2193s, Maps update=0.0039s pub=0.0169s (local map=27, WM=27)
[ INFO] [1672085186.818871764, 1671607358.598912398]: Assembled 1 obstacle and 1 ground clouds (49102 points, 0.017088s)
[ INFO] [1672085186.821994301, 1671607358.614195911]: rtabmap (32): Rate=1.00s, Limit=0.000s, Conversion=0.0034s, RTAB-Map=0.2123s, Maps update=0.0033s pub=0.0202s (local map=28, WM=28)
[ INFO] [1672085187.674891590, 1671607359.459863750]: Assembled 0 obstacle and 0 ground clouds (49102 points, 0.000022s)
[ INFO] [1672085187.675098944, 1671607359.459863750]: rtabmap (33): Rate=1.00s, Limit=0.000s, Conversion=0.0017s, RTAB-Map=0.0992s, Maps update=0.0017s pub=0.0002s (local map=28, WM=28)
[ INFO] [1672085188.723197049, 1671607360.514604766]: Assembled 0 obstacle and 0 ground clouds (49102 points, 0.000025s)
[ INFO] [1672085188.723318539, 1671607360.514604766]: rtabmap (34): Rate=1.00s, Limit=0.000s, Conversion=0.0018s, RTAB-Map=0.1275s, Maps update=0.0024s pub=0.0002s (local map=28, WM=28)
[ INFO] [1672085189.680629017, 1671607361.467686535]: Assembled 0 obstacle and 0 ground clouds (49102 points, 0.000018s)
[ INFO] [1672085189.680748647, 1671607361.467686535]: rtabmap (35): Rate=1.00s, Limit=0.000s, Conversion=0.0018s, RTAB-Map=0.1100s, Maps update=0.0020s pub=0.0001s (local map=28, WM=28)
^C[apriltag_ros_continuous_node-11] killing on exit
[rviz-10] killing on exit
[robot_state_publisher-9] killing on exit
[joint_state_publisher-8] killing on exit
[rtabmap/rtabmap-7] killing on exit
[base_to_vlp16_tf-6] killing on exit
[pose_to_base_tf-5] killing on exit
[base_to_d415_tf-4] killing on exit
[base_to_t265_tf-3] killing on exit
[base_to_d435_2_tf-2] killing on exit
[base_to_d435_1_tf-1] killing on exit
rtabmap: Saving database/long-term memory...
rtabmap: Saving database/long-term memory...done!

This is the tf_echo output:

At time 1671607328.118
- Translation: [3.562, -0.384, 1.396]
- Rotation: in Quaternion [0.518, -0.503, -0.478, 0.500]
            in RPY (radian) [1.615, -0.008, -1.534]
            in RPY (degree) [92.510, -0.443, -87.917]
At time 1671607329.058
- Translation: [3.584, -0.360, 1.392]
- Rotation: in Quaternion [0.526, -0.513, -0.469, 0.491]
            in RPY (radian) [1.650, -0.010, -1.536]
            in RPY (degree) [94.534, -0.600, -87.996]
At time 1671607330.066
- Translation: [3.713, -0.046, 1.391]
- Rotation: in Quaternion [0.570, -0.476, -0.422, 0.520]
            in RPY (radian) [1.674, -0.014, -1.379]
            in RPY (degree) [95.898, -0.808, -79.001]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607330.537
- Translation: [3.766, 0.279, 1.389]
- Rotation: in Quaternion [0.604, -0.437, -0.377, 0.550]
            in RPY (radian) [1.681, -0.026, -1.230]
            in RPY (degree) [96.336, -1.487, -70.479]
At time 1671607356.168
- Translation: [2.952, -0.764, 1.375]
- Rotation: in Quaternion [-0.504, 0.538, 0.489, -0.466]
            in RPY (radian) [1.659, -0.008, -1.627]
            in RPY (degree) [95.032, -0.483, -93.230]
At time 1671607357.104
- Translation: [3.400, -0.445, 1.385]
- Rotation: in Quaternion [0.519, -0.500, -0.479, 0.502]
            in RPY (radian) [1.609, -0.004, -1.528]
            in RPY (degree) [92.186, -0.227, -87.573]
At time 1671607358.173
- Translation: [3.780, -0.287, 1.386]
- Rotation: in Quaternion [-0.484, 0.624, 0.504, -0.349]
            in RPY (radian) [1.822, 0.053, -1.863]
            in RPY (degree) [104.386, 3.013, -106.765]
At time 1671607359.141
- Translation: [3.898, -0.240, 1.384]
- Rotation: in Quaternion [0.536, -0.477, -0.459, 0.524]
            in RPY (radian) [1.601, -0.008, -1.448]
            in RPY (degree) [91.721, -0.444, -82.959]
At time 1671607360.144
- Translation: [3.899, -0.240, 1.385]
- Rotation: in Quaternion [0.528, -0.483, -0.471, 0.516]
            in RPY (radian) [1.595, -0.001, -1.479]
            in RPY (degree) [91.374, -0.071, -84.760]
At time 1671607361.114
- Translation: [3.896, -0.240, 1.384]
- Rotation: in Quaternion [0.532, -0.476, -0.465, 0.524]
            in RPY (radian) [1.589, -0.003, -1.456]
            in RPY (degree) [91.016, -0.196, -83.422]

Even if I see the tags in rviz and they are published correctly according to tf_echo, they don't get added to the map because no green links show up in the occupancy grid map.

[matlabbe]

You could try to check if the tag callback is called by adding a ROS_WARN in https://github.com/introlab/rtabmap_ros/blob/507df071e09fd52abbd6a31474a4d9a2bd1035ee/src/CoreWrapper.cpp#L2317

The TF is not always published, only on detections, so it is normal that tf_echo would fail when the tag is not detected.