How to use Apriltags to transform the map frame

[schmiran]

Hello everybody,

I am a bit stuck and hope one of you can help me.

I have a system with a T265, two D435, a D415 and a VLP-16 running and the mapping is working fine. I would like to overlay the generated map with a CAD model afterwards. Therefore I would like to output the point cloud in the model/world coordinate system. For this I have hung up Apriltags in my test area and know their position in the model/world coordinate system. I detect the markers with an external apriltag_ros node.

My plan now was to move the map_frame based on the detected Apriltag position. I added a static_transformation from world -> map (all 0.0, because this transformation must be determined depending on the detected tag position) and a static_transformation tag -> world. My hope was that the map_frame would then shift accordingly with the world_frame. Unfortunately this is not the case. On the screenshot you can see my TF positions. The sensors, the tag and the world are at the right position, but the map frame is not moved.

Now my question, what do I have to do to get the map frame moved to the world frame or is there a better way to output the point cloud in the world coordinate system?

I am running rtabmap and rtabmap_ros 0.19.3.

Thanks in advance! schmiran

[matlabbe]

Can you try to change the Fixed frame in rviz's Global options to world instead of map?

EDIT: The TF between world and tag is wrong, because it will break the tf tree. If you know the tag position in world, adjust the transform between world->map, so that world->tag (not published) = world->map->odom->base_link->d415_link->tag

[schmiran]

Hello matlabbe

thank you for your answer. If I change the fixed frame to world, I still get the point cloud displayed, but that only changes the visualization and unfortunately not the output point cloud. Partially I get then also an error in the TF tree displayed, as you have also already noted.

I understand what you say in your edit and that makes sense to me. But it is not clear to me, how I can modify the transformation from world -> map in dependence of the recognized position of the tag (and the knowledge about the real position) in such a way that the tree fits then from tag up to the map. Do you have a tip for this? I have so far only added static TF and otherwise used the TF published by RTABMAP and other nodes.

Greetings schmiran

[matlabbe]

If you don't detect tags, what is the TF tree? In your screenshot, the /base_link frame is not a child of /odom and /map frames.

If you have a standard tree like this: /map -> /odom -> /base_link -> /camera_link -> /tag

T_map_tag = T_map_odom x T_odom_base x T_base_camera x T_camera_tag

So based on this equation: T_world_tag = T_world_map x T_map_odom x T_odom_base x T_base_camera x T_camera_tag

and that T_world_tag is known, then: T_world_map = T_world_tag x inverse(T_map_odom x T_odom_base x T_base_camera x T_camera_tag)

[schmiran]

Thanks for your detailed answer. In my screenshot here you can see that I have the setup you describe, even though I recognize the tag (I don't know why the sub TF are only shown on the D415 here, but the others are there too).

I think there is a misunderstanding. I know how to mathematically calculate the transformation, but I don't know how to get rtabmap to output its results transformed by this transformation. Is there an elegant way to do this?

My approach would be to transform all results of rtabmap (and the results of other nodes that refer to the map frame) accordingly in my post-processing, but for that I would have to save the TF tree somehow, which I don't do at the moment and don't know how to do.

EDIT: So the easiest way would be to manipulate the map frame so that it would be transformed according to the discussed transformation map -> world. Then all results should be transformed accordingly in my understanding.

[matlabbe]

Yes, you just have to adjust world->map to move the map accordingly to tag detected.

Note that TF world->Tag should not exist, because tag will have two parents (the camera and world). It should only have the camera as parent.