Would a feature to integrate fixed frame poses (GPS-like measurements) be useful?

[3473f]

I am currently investigating a feature to integrate fixed frame poses into cartographer_ros and was wondering if it would be interesting to the community.

I see two uses cases for the feature:

• If you are provided with a dxf map with the static features for the area, so you can localize using such a map to provide global constraints to Cartographer.
• If you have landmarks with know global poses, so as long as a landmark is seen, it will provide a fairly good global pose estimate to Cartographer.

These two estimates are in the form of geometry_msgs/PoseWithCovarianceStamped but the covariance doesn't seem to be used in fixed_frame_pose_data.h, so would it make more sense to publish them as a geometry_msgs/PoseStamped instead? In case of a geometry_msgs/PoseWithCovarianceStamped, one can implement a fixed_frame_pose_covariance_threshold to handle bad pose estimates. If the threshold exceeded, the information is simply ignored.

Another point is handling intermittent measurements (example: when we don't see any landmark). My current understanding is that Cartographer expects to receive the messages all the time even if we have a measurement drop, would passing an empty pose message be sufficient or it has to be integrated somehow in the message? maybe setting the covariance to a large value.

[johuber]

Another point is handling intermittent measurements (example: when we don't see any landmark). My current understanding is that Cartographer expects to receive the messages all the time even if we have a measurement drop, would passing an empty pose message be sufficient or it has to be integrated somehow in the message? maybe setting the covariance to a large value.

As a first note: this statement is not correct, there is the option TRAJECTORY_BUILDER.collate_landmarks, if you set this to false landmarks to not have to arrive at a fixed rate.

How exactly would what you suggest differ from the current landmarks? Your two use cases seem to be fully covered..

[3473f]

How exactly would what you suggest differ from the current landmarks? Your two use cases seem to be fully covered..

In the current implementation, the global pose of the landmarks within the map is inferred by cartographer given a relative pose measurement. While my use case can be fulfilled using the current implementation of landmarks, this global pose is subject to error if the vehicle's pose estimation is erroneous. (correct me if I am getting this wrong)

In my first case, I have an initial dxf map of the environment in which I am localizing using MCL. I know the transform between the dxf map and cartographer's map (from the initial pose in the dxf map since cartographer starts at (0, 0, 0)), so I can provide my vehicle's pose in cartographer's map directly. The idea is to use this pose as an extra global constraint when its covariance is low enough. In the second case, we are detecting features in the environment with known global poses. In these two cases the poses are inferred by an external system and not cartographer itself.

Anyway, I have implemented the fixed frame pose interface and will be recording some data with ground truth and testing using it. I will also compare with the current landmark implementation and check if there is any benefit to doing it the other way.

[johuber]

How exactly would what you suggest differ from the current landmarks? Your two use cases seem to be fully covered..

In the current implementation, the global pose of the landmarks within the map is inferred by cartographer given a relative pose measurement. While my use case can be fulfilled using the current implementation of landmarks, this global pose is subject to error if the vehicle's pose estimation is erroneous. (correct me if I am getting this wrong)

I am not fully sure if you are getting it right. The landmark will just add a constraint to the optimization problem, hence irrespective of the vehicle's pose estimation it will use the landmark measurement to improve the pose estimation of the vehicle. In pure localization mode the position of the landmark will be fixed in the map frame, in SLAM mode it will be optimized similarly to how the entire map is optimized.

In my first case, I have an initial dxf map of the environment in which I am localizing using MCL. I know the transform between the dxf map and cartographer's map (from the initial pose in the dxf map since cartographer starts at (0, 0, 0)), so I can provide my vehicle's pose in cartographer's map directly. The idea is to use this pose as an extra global constraint when its covariance is low enough. In the second case, we are detecting features in the environment with known global poses. In these two cases the poses are inferred by an external system and not cartographer itself.

I think even in this case it could make sense to formulate it as it is currently, since you probably also don't have full certainty of the transform from your "MCL map frame" to the "cartographer map frame". So basically the origin of your "MCL map" is the landmark your observing, then you can use the same formulation as now, where the tracking_from_landmark_transform entry in the landmark represents the transformation from the "MCL map" to the "robot pose" (which is exactly your MCL pose)

Does that make sense?

[3473f]

The landmark will just add a constraint to the optimization problem

won't the constraint for a landmark (Lk) be added relative to an interpolated pose (Po) between two other nodes (Pi & Pj) on the graph?

........Lk .........| Pi --- Po --- Pj

so the landmark is initially "fixed" to a global pose based on the vehicle's relative observation and hopefully once the graph has been optimized the landmark will end up where it actually is in the global frame (within a margin of error) and could be later used in pure localization mode. Here, the information of the landmark's global pose is not used. Our idea is to use MCL/Landmarks to aid the mapping process (by adding global constraints through these measurements) as our specific setup has two low-range sensors (8 meters) in some open areas and VERY bad odometry. So the high odometry weights aid in the tricky situations but result in a bad constraints over the entire graph.

Alternatively, I can try to pass the MCL estimate as odometry (sort of a initial sensor fusion step) and the transform between the two frames won't matter as only the odometry deltas are used, and finally use the current landmarks implementation with the landmarks.

Does that make sense?

it does! thanks for the help. I will be reporting back with the results in a few days.

[johuber]

so the landmark is initially "fixed" to a global pose based on the vehicle's relative observation and hopefully once the graph has been optimized the landmark will end up where it actually is in the global frame (within a margin of error) and could be later used in pure localization mode. Here, the information of the landmark's global pose is not used. Our idea is to use MCL/Landmarks to aid the mapping process (by adding global constraints through these measurements) as our specific setup has two low-range sensors (8 meters) in some open areas and VERY bad odometry. So the high odometry weights aid in the tricky situations but result in a bad constraints over the entire graph.

You are right, this is a bit different if you already want to use your MCL during mapping! So an implementation for this could make sense in your use case; however, I am not sure if it is a very common one, since you assume to have a "perfect map" already from MCL and then remap the area using cartographer.