The map transformation induced by Markers/Priors is wrong/jumps around

[mrtnbm]

Hello!

Everything is working fine now without Marker/Priors — markers get detected and added to the map. Unfortunately, when I activate Marker/Priors, the transform between map and odom_frame jumps around.

Coordinate setup

These are the global coordinates of the test environment with the coordinate origin measured (x pointing away from tag1, z pointing towards tag2 and y is pointing up to the ceiling:

Parameter used in launch file

In order to set these global coordinates, I used the following Marker/Priors parameter value (assuming that tags have the coordinate system described in https://github.com/AprilRobotics/apriltag/wiki/AprilTag-User-Guide#coordinate-system:

The tag's coordinate frame is centered at the center of the tag, with x-axis to the right, y-axis down, and z-axis into the tag.

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

That's why I calculated the global coordinates/angles for the transformations between my coordinate system origin in the lab environment and the Apriltags coordinate system. (Is this right?) In my opinion this defines the correct coordinates/orientations in the global coordinate system respecting the Apriltag coordinate system, right?

Output

Unfortunately, the coordinate origin is wrong in the resulting map and it jumps around whenever tag2 is found and immediately back at the time of tag1 detection. This means the relative position/orientation between both tags is wrong, but I confirmed that I'm using the right global coordinates/orientation — see also this video here of RViz output that visualizes this problem:

rviz_roslaunch_rtabmap_markerdetect_2.mp4

As soon as marker 2 gets detected, the transform changes completely. (unfortunately this recording does not stop at the marker, because while being stopped the marker was not fully visible for the camera, that's why it only gets detected in a moving state which probably also degrades accuracy but it shouldn't jump that much).

Database file

Microsoft OneDrive - rtabmap.db (https://1drv.ms/u/s!AhXLzuMKDjgkx2wcoJRv_osdQYsc?e=n8sZCi)

Best regards
Martin

[matlabbe]

I think the marker priors are wrong.

If we set Optimizer/PriorsIgnored to true to ignore the priors, they are where they are estimated:

By mouse over the nodes in Graph view, we can see that the markers are located at (x y z roll pitch yaw):

Marker 1 
Estimated :  3.59 0.39 1.18 0     0 -1.56
Your prior: -3.56 1.58 0.50 0 -1.57  3.14159

Marker 2 
Estimated :   1.56  4.04 1.33 0.13 0.05  -0.09
Your prior: -1.495 1.605 4.91    0    0   3.14159

Converting your tags location from world frame to ROS frame (x->forward, y->left, z->up), we would get:

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

Here the new optimized map with those parameters (note that the map frame is not anymore on the first node of the graph):

And markers location are now:

Marker 1 
Estimated : 3.57 0.50 1.18  0.002 -0.008 -1.55
New prior:  3.56 0.50 1.583 0      0     -1.5708

Marker 2 
Estimated : 1.49  4.91 1.33 0.13 0.05 -0.07
New prior:  1.495 4.91 1.605 0     0     0

This is closer, though Z values ar off as the whole map is a little tilted. I see Reg/Force3DoF=true and Marker/PriorsVarianceLinear=0.001 and Marker/PriorsVarianceAngular=0.001 I noticed that the odometry is 6DoF, which can make graph optimization weird if optimization is limited to 3DoF.

The static tf between base and camera/lidar links may not be correctly configured. Here the lidar looks tilted in comparison to base frame:

Beware when you rotate, you may skew the lidar, which makes difficult for ICP to compute the real motion:

See test_velodyne_t265_deskewing.launch or test_velodyne_d435i_deskewing.launch for examples to deskew lidar data with IMU.

[mrtnbm]

Hello Mathieu,

thank you so much. The Priors are working now. I didn't know that I still have to transform it into the ROS coordinate system, but of course that makes sense.

What do I have to do to keep a user-defined coordinate system? Can I transform the axes based on my lab coordinate system? The origin of the coordinate system is now correct, but how do I get the correct directions for the axes? Is this possible in rtabmap with markers/priors or do I have to change this manually?

Regarding the Z value issues:

This is closer, though Z values ar off as the whole map is a little tilted. I see Reg/Force3DoF=true and Marker/PriorsVarianceLinear=0.001 and Marker/PriorsVarianceAngular=0.001 I noticed that the Odometry is 6DoF, which can make graph optimization weird if optimization is limited to 3DoF.

How do I force 3DOF on Odometry, in order to fix the issue with the z values and also get better graph optimization?

The static tf between base and camera/lidar links may not be correctly configured. Here the Lidar looks tilted in comparison to base frame

How did you manage to get this output for debugging? Is this in RViz or RTABMap Viewer?

See test_velodyne_t265_deskewing.launch or test_velodyne_d435i_deskewing.launch for examples to deskew lidar data with IMU.

Thank you for these examples.

[matlabbe]

In rtabmap-databaseViewer, look at the 3D View, Graph View and Constraints View panels.

If you are using rtabmap's odometry nodes, you can set Reg/Force3DoF to true for them.

If you want to transform the whole map in your user coordinate frames other than ROS frame, that would be done outside ROS after exporting the cloud. I would suggest to stick with ROS standards (REP105) to avoid issues with all ROS packages.

[mrtnbm]

Hello Mathieu,

thank you for clarifying!

I will close this issue as soon as I have gone through everything again to check if I have no more questions.

Best regards
Martin

[mrtnbm]

Hey Mathieu,

sorry for reopening this again - just to be clear: Marker/Priors only does the translation part of the transformation but not the rotation? Is that true?
Is there any way to do the rotation into the Lab Coordinate System in ROS, because I don't need to import the Pointcloud into ROS anymore afterwards which would prevent issues with different coordinate systems?

[mrtnbm]

Alternatives I considered:

1. If I want to include rotations by manually editing the source code to add this feature at the position where the translations are calculated for Markers/Priors, where do I look? What file and function do I need to manipulate to add the rotation feature based on my lab coordinate system to Markers/Priors?

2. Is there a way to export the transformations between map_frame and odom_frame from Marker/Priors to use in external software for handling of custom coordinates representations?

3. Can I use the earth_frame to get the translation and rotation working? As far as I know, the earth_fame is not subject to the constraints of REP 105 like the map frame, since it is designed for simultaneous use with multiple robots performing multiple scans. (I would still use it for only one robot, but this may be better than not following REP105 by rotating the map_frame?)
   Is there a way to add an earth_frame when using Marker/Priors that holds the information about my custom coordinate system with changed rotation/axes?

4. Can I guarantee by modifying the frame that a fixed coordinate direction is always present in the map_frame, based on the cardinal directions? For example, the x-axis always points to the north , y always points to the east,...
   For better visualization what I want to do as an alternative, I will quote this comment from https://answers.ros.org/question/382051/the-relationship-of-earth-frame-and-map-frame/:

   If you build a map with `rtabmap' and save it, you can then calculate the static rotation needed based on where the map x-axis ended up. Doing this dynamically is more difficult: you will need to read the initial magnetic orientation of the robot once before you begin the rtabmap mapping procedure.

   Would that work to get the right rotation from the RTABMap ROS coordinate system into my coordinate system, while the translation is done with Marker/Priors?

[matlabbe]

The priors should be in ROS coordinate frame. If you want an arbitrary world frame with Y up, you can add a static transform publisher between /world frame and /map frame.

rosrun tf static_transform_publisher 0 0 0 0 -1.57 0 world map 100

The priors can make the whole map rotate. For example with:

1 -3.56 -0.50 1.583 0 0 1.5708|2 -1.495 -4.91 1.605 0 0 3.1459

I think the problem in your case is that the Priors are not using the same coordinate frame than the odometry (which uses x->forward, y->left, z->up by default). If you transform the odometry in your world frame, you may use the priors as you set them.

The Marker/Priors could include orientation in earth frame, so the resulting map will follow earth frame coordinates.

[mrtnbm]

Hello Mathieu!

The priors can make the whole map rotate. For example with [...]

Oh, I think I was utterly confused the last days and somehow missed this feature. So Marker/Priors actually ensures a consistent orientation of the coordinate system axes - unlike what I thought the days before.

Here my different starting positions without priors:

And with priors enabled:

Both graphs with different starting points (and markers) actually have a consistent coordinate system now, thank you very much for helping!

It is greatly appreciated that you invest so much time into this project, Mathieu. This definitely helps to finish my project.
I'm closing this for now, everything is working as expected.

[ZitongLan]

Hello Mathieu! I know this may be really ergent. But is there a way to export the tag's information out? Like their id, xyz and facing data from the graph view together? Rather than mouse over. @matlabbe

[matlabbe]

If you are using ROS:

rostopic echo /rtabmap/mapGraph
rosservice call /rtabmap/publish_map 0 1 1

With the standalone, we would need to add an option in the Export poses... menu action so that tags could be added but with 0 timestamps.

[ZitongLan]

@matlabbe Thanks for your timely reply! Does this mean that I need to launch rtabmap.launch to start the node and also start rostopic echo /rtabmap/mapGraph and rosservice call /rtabmap/publish_map 0 1 1 ? Could I directly using some command to export the tags location from the standalone program?

Update:
When I use rtabmap standalone and do rosservice all /rtabmap/publish_map 0 1 1. It returns me a error that service [rtabmap/publish_map] is not available

[matlabbe]

See introlab/rtabmap#1199 for standalone, new option has been just added.

For ROS, a complete example would be:

roscore
rosrun rtabmap_slam rtabmap _database_path:=~/Downloads/map_with_tags.db
rostopic echo /mapGraph
rosservice call /publish_map 0 1 1

Output of the echo (landmarks have negative ids):

...
posesId: [-17, -13, -12, -10, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 24, 25]
poses: 
  - 
    position: 
      x: 0.8192823529243469
      y: 0.48825740814208984
      z: 0.01175188459455967
    orientation: 
      x: -0.4414244498204657
      y: -0.4967600249685705
      z: 0.563409453797233
      w: 0.49086031612682146
  - 
    position: 
      x: 0.5227816700935364
      y: 0.8374179005622864
      z: 0.01531982235610485
    orientation: 
      x: -0.1990972037388025
      y: -0.63058735593545
      z: 0.7136995181266821
      w: 0.23098252425566432
  - 
    position: 
      x: 0.9040536880493164
      y: -0.15140362083911896
      z: 0.013636903837323189
    orientation: 
      x: -0.48105965363837866
      y: -0.40019350338574644
      z: 0.5032363608352053
      w: 0.595969654476892
  - 
    position: 
      x: 0.8321346640586853
      y: -0.53339684009552
      z: 0.0184736717492342
    orientation: 
      x: -0.6050949751557956
      y: -0.30175370491570086
      z: 0.33409219354576186
      w: 0.6566483801248193
  - 
    position: 
      x: -1.0634676250731445e-08
      y: -4.029799161031633e-09
      z: -2.8044946365213264e-09
    orientation: 
      x: -0.01138162580725946
      y: -0.040386872408596126
      z: 0.0003231435081192674
      w: 0.999119244080592
...