Fix tf transforms in case of Area Learning/COM

[smits]

AFAIU, when the area learning frame is going to be used (COM or when we load an ADF) we should publish the device pose wrt the area learning frame instead of the Start of Service frame.

In case of VIO: /start_of_service --> /device In case of COM/AL: /start_of_service --> /area_learning --> device

As long as we are not localised yet /start_of_service --> /area_learning will be identity. When we loose localisation we should stick to the old /start_of_service --> /area_learning until being relocalised instead of starting to publish wrt /start_of_service.

[dvanthienen]

Some perspective wrt. ROS: The ROS navigation stack has as TF tree map->odom->base_link Where odom->base_link is published by ekf_localisation based on encoder and IMU data, and map->odom is published by amcl based on laserscan and map information. In reality amcl estimates map->base_link, but since TF can only handle tree structures, it substracts odom->base_link from it. It is an ugly fix for an even uglier problem: actually the laserscan, encoders and IMU all give localisation information. An odom frame that keeps track of drift from the start doesn't make sense. A nicer solution should be the sensor fusion of all three the localisation information sources to a single estimate of the base_link wrt. the map. If no map exists, the map frame will be simply the starting location. Odometry drift does only exist since the last sensor fusion with a sensor source with less covariance, eg. the laserscan or point cloud.

Now, let's go back to the Tango to ROS lingo mapping:

• start_of_service ~ odom
• area_learning ~ map
• device ~ base_link (or at least a fixed transformation apart from it)

Hence this proposal does not map on the typical ROS layout. In itself not a problem, however it makes mixing information sources more difficult, eg. using ekf_localisation based on robot IMU and encoders (odom->base_link) combined with Tango localisation.

My questions:

• Would it be possible to consider the IMHO theoretical more sound sensor fusion option?
• If not, then why lose consistency with ROS?

[adamantivm]

Here is some additional reference on how odom, map and base_link are used in ROS: http://www.ros.org/reps/rep-0105.html#coordinate-frames

[dvanthienen]

After some more thought: actually start_of_service != odom. Odom wrt. base_link in ROS is updated with the odometry (encoder+IMU) updates, start_of_service wrt. map is only updated when a loop closure is happening, it is not an output of VIO AFAIK. Hence maybe a more suitable proposal: start_of_service -> area_learning = map -> odom -> base_link OR area_learning = map -> odom -> start_of_service -> base_link And then probably the first is the easiest to mix with the ROS navigation stack. In case of using tango_ros only, map=odom.

[dvanthienen]

Tango frame relations:

ROS frame relations: AMCL calculates this TF loop closure to make TF work.

ROS and Tango frame relations:

In a perfect world with ROS and Tango map alignment and starting in the map frame, start_of_service, odom, map and area_description align. In a even more perfect world only map, base_link and device frame exist, with map -> base_link defined by decent sensor fusion of all underlying information sources.

[dvanthienen]

After some more discussions and the insights of the figures above, a new iteration: To project these relations in the linear structure needed in TF:

• Choosing the root: map makes most sense since this is the model of your world, as is the choice in ROS in general.
• Assuming fixed offset between device and base_link
• Considering the 'use all possible information' case, where both robot and tango sensor data is used all together without decent sensor fusion
• Structure based on typical update frequencies from slow to fast: map updates (<1Hz) -> map based localisation (about 1-10Hz) -> VIO (30Hz) -> encoder info (about 100Hz)

=> map -> area_description (AD) -> start_of_service (SoS) -> odom -> base_link~device

• The first arrow is defined by calculating map_T_ad = map_T_base_link (from AMCL) * base_link_T_ad (from TF).
• The second arrow is defined by calculating ad_T_sos = ad_T_device( from tango localisation) * device_T_sos (from TF/tango).
• The third arrow is defined by calculating sos_T_odom = sos_T_device (from tango VIO)* device_T_odom (from TF)
• The fourth arrow is defined by ekf_localization.

When there is no ekf_localisation => SoS = odom When there is no AMCL => map = AD (+fixed offset)

[PerrineAguiar]

@dvanthienen FYI, here you can find the tutorial about using rtabmap with tango, in which the issue with _areadescription vs map is mentioned:

"_Note that to not screw up the TF tree, we would have to set odom_frame_id:="area_description" because there is a competition between /area_description -> /start_of_service and /map -> /start_of_service. So we would have instead /map -> /area_description -> /start_of_service, which seems not correct as two modules correct odometry. I didn't see the option to remove that frame (/areadescription), which seems related to enabled drift correction of Tango by default."

This is why we started to think that our tango tf tree was wrong wrt. the ros tf tree, and that we needed to swap _start_ofservice and _areadescription.

[smits]

My 2 cents:

• In both the Tango and ROS world, area_description and map are fixed to the world, so area_description ~ map (there might be a fixed offset involved here).
• In case of loop closures, either in Tango or by AMCL start_of_service and odom get updated wrt to their respective area_description/map, since both use different sensor data and algorithms they represent the same frame, but are not necessarily the same, hence we should/could do proper fusion here.
• Tango VIO and ROS EKF localisation provide device or base_link which are/should be physically fixed together. Also here we have two different sources for the same frame. So proper fusion should be done here, probably by using the Tango data as additional input to the EKF localisation or adding an additional KF after this.

I wouldn't spend time in a solution without some proper sensor fusion.

[dvanthienen]

As stated above, I do agree. It is about finding a reasonable structure in TF that can deal with all scenarios, even the ugly ones. If you do it properly, you don't need intermediate frames and you can leave them out, but if you don't, like most guys probably, some guidelines are handy.

[dvanthienen]

@PerrineAguiar I don't understand their issue. What two modules correct odometry? and what competition are they referring too?

[PerrineAguiar]

I guess the two modules that they are mentioning are the drift correction of Tango and the RTAB-Mapping of ROS, they are both "competing" to correct odometry. AFAIU RTAB-Map is doing something similar to Tango drift correction, and I guess they wanted to use Tango only to provide the point cloud and the color image. So maybe switching off Tango drift correction would be enough in their case.

EDIT: Just found this post mentioned in the tutorial. The author of the tutorial is explaining the issue more in detail:

_/map -> /start_of_service is published by rtabmap, it is the drift error estimated by RTAB-Map's loop closure detection. /area_description -> /start_of_service is the drift error estimated by Tango's loop closure detection (when Tango drift correction is enabled). The TF error is that we cannot have a frame (/start_of_service) with two parents (/map and /area_description). Tango with drift correction does the same job as rtabmap node: correcting odometry drift on loop closure. A solution would be to disable Drift correction from Tango ROS Streamer, so that /area_description is not published. Just saw in Tango ROS Streamer repo that there would be an option to use Tango in Odometry mode only, which is the mode we would want with rtabmap (config_enable_driftcorrection=false).

[dvanthienen]

Indeed, or they just disable AD, or they align their TF tree in the way I proposed above: map -> area_description (AD) -> start_of_service (SoS)

[dvanthienen]

Anyway, providing the option to disable Tango publishing to tf directly and adding the option to just publish the relevant transformations as a geometry_msgs/TransformStamped allows the user to write a tf broadcaster themselves.