map frame is transformed against odom instead of the opposite

[ricber]

Hi @koide3 , I have noticed that the loop closure corrections shift the map frame against the odom frame. Is this by design? Usually, the map frame is static and the corrections from loop closures are applied shifting the odom frame relative to the map frame. Shifting the map frame causes the final map to have a weird translation and rotation relative to the ground. You can see the effect in the following screenshot.

In my opinion, I think that this problem is by design. Indeed, I see that, when the enable_floor_detection parameter is true, you are doing the same. In fact, you translate the map frame to have the x-y plane aligned with the ground, while the odom frame stays fixed. I think that in this case, it could make sense to translate map against odom. However, when no constraints are activated, translating map against odom is not what users are expecting.

I have already reported the issue here #95, however, I was wrong. The map is saved correctly, the problem is the transform between map and odom.

[koide3]

Hi @ricber ,

What I implemented is something similar to the one in amcl, in which the map-odom transformation compensates for odometry errors and keeps base_link in the correct position on the map (see 3.1.6 at http://wiki.ros.org/amcl). So, shifting map against odom itself is by design. Everytime a new constraint is added, the map frame can be shifted against the odom frame.

But, the map frame should not be shifted against the odom frame when no constraints are made. If you see such a shifting phenomenon, there may be a problem in the map-odom transformation handling. I try to fix this problem.

Edit: I misunderstood what you meant. Because a tf tree must have a single root, we cannot choose the opposite odom w.r.t. map transformation direction. Anyways, if the program properly transforms points in the map frame during the saving process, the map should be aligned to the XY plane in the PCD file. I will try to find what is wrong in the implementation.

[ricber]

When you say that no constraints are made... isn't the loop closure a constraint added to the pose graph? Thus, it is normal to see a shift in the transformation between map and odom when the graph is optimized.

Also, if the tf tree must have a single root, the directions of the transformations map->odom and odom->base_link should be coherent. That is, or you transform the base_link relative to odom and odom relative to map, either you transform odom relative to the base_link and map relative to odom. Is this true in your code? And which one of these two approaches are you using?

EDIT: The thing that was confusing me is the fact that in the hdl_graph_slam_nodelet.cpp file you are using an odom2map publisher. This is why I said that you were publishing odom against map. Actually, I've just seen that, although the name, you are correctly publishing the odometry from map to odom.

EDIT 2: This line of code looks strange: https://github.com/koide3/hdl_graph_slam/blob/0331eec84007fc783aaaeb2fdab8d93cf7ef1a98/apps/hdl_graph_slam_nodelet.cpp#L608 You are publishing odom2map transformation but in the matrix2transform function, map is the parent frame_id, while odom is the child frame_id. https://github.com/koide3/hdl_graph_slam/blob/0331eec84007fc783aaaeb2fdab8d93cf7ef1a98/apps/hdl_graph_slam_nodelet.cpp#L594