How to check localization quality

[francescoricciuti]

Dear community,

I'm trying to automate as much as possible the localization procedure on my platform, and for this reason I would need a measure of the "localization quality" to start autonomous navigation only when we are correctly localized in our map.

I have read a bit on this topic, and I understand that localization quality, in general, is difficult and probably meaningless to define in this framework, but still I need some metrics that define the current state of the algorithm.

I was able to understand when the robot localizes in the map when the algorithm starts using the metrics provided by Cartographer, in particular using the local/global constraints searched/found.

What I am trying to do now is to understand if the current submap deviates from the map while running the algorithm.

Is there any metric in particular already giving this information?

Is there someone who has already implemented something similar?

Thanks in advance, Francesco

[MichaelGrupp]

Is there someone who has already implemented something similar?

:wave:

I was able to understand when the robot localizes in the map when the algorithm starts using the metrics provided by Cartographer, in particular using the local/global constraints searched/found.

This sounds reasonable, but keep this in mind: this mapping_constraints_constraint_builder_{2d, 3d}_constraints metric is ever-increasing over the runtime, regardless of trimming/deleting submaps.

Some things you can do:

• mapping_constraints_constraint_builder_{2d, 3d}_constraints: e.g. calculate ratio of found vs. searched constraints (ideal: 1.0), over the whole runtime.
• mapping_constraints_constraint_builder_{2d, 3d}_scores: this is a histogram of the matching scores that were found over the whole runtime. You can do statistics based on that.
• mapping_{2d, 3d}_pose_graph_constraints: the current number of constraints in the pose graph. This metric is aware of trimming/deletion. If the number of tag==inter_submap, trajectory==different constraints is >0, you have found a "localization match". trajectory==same are loop closure constraints.

What I am trying to do now is to understand if the current submap deviates from the map while running the algorithm. Is there any metric in particular already giving this information?

As I wrote above, the constraint scores would give you a hint of the matching score distribution over the whole runtime. What's currently missing is a live metric that only takes the currently active trajectory into account if we are not interested in historical data. This wouldn't be possible with a histogram, but we could have a gauge family with e.g. the current min/max/mean constraint score of the active trajectory?

I have read a bit on this topic, and I understand that localization quality, in general, is difficult and probably meaningless to define in this framework

Yes, it's difficult. If you have any additional information outside Cartographer that you can use to measure the quality of the estimate, consider using that too.

[francescoricciuti]

Hi Michael, Thanks very much for your quick answer!

mapping_constraints_constraintbuilder{2d, 3d}_constraints: e.g. calculate ratio of found vs. searched constraints (ideal: 1.0), over the whole runtime.

mapping_constraints_constraintbuilder{2d, 3d}_scores: this is a histogram of the matching scores that were found over the whole runtime. You can do statistics based on that.

Yes, this is something I've been trying to do. My problem here is that I didn't manage to have a reliable score based on these information, probably because of my poor comprehension of the localization's underlying logic.

From what I see, it doesn't look for constraints at a constant rate, and doesn't always look for both local and global constraints. I've implemented a logic which gives priority to local constraints, because from my understanding those are the ones that are searched more frequently when we are localized. In the current implementation, I'm computing the average score from mapping_constraints_constraint_builder_3d_scores (probably would be better to compute a moving average to exclude score old in time), and I'm computing a penalty term based on how often the system looks for a constraint and doesn't find it. Unfortunately this approach is not giving robust results for the moment, do you think what I'm doing has some sense (at least from the theoretical point of view) ?

mapping_{2d, 3d}_pose_graph_constraints: the current number of constraints in the pose graph. This metric is aware of trimming/deletion. If the number of tag==inter_submap, trajectory==different constraints is >0, you have found a "localization match". trajectory==same are loop closure constraints

Great, I didn't consider this metric up to now. So if I check the inter constraints on different trajectories I should be able to understand if it found a "localization match" between my current submap and the overall map?

but we could have a gauge family with e.g. the current min/max/mean constraint score of the active trajectory?

Yes, this might be very usefull. I know that without an external ground truth it is impossible to have an absolute score, but the approach with the constraint score would at least give an idea to implement a "ask help" functionality during autonomous navigation.

The last thing that I've been trying is to use mapping_3d_local_trajectory_builder_{scores,cost,residuals} to have some additional informations on the local scan matching, but I don't understand what these metric really mean: are these scores computed in a scan match between the current submap and the saved overall map on which I'm trying to localize or these scores are computed in the local SLAM procedure while actually building the submap?

Thanks very much for your precious help!

Best, Francesco

[MichaelGrupp]

it doesn't look for constraints at a constant rate, and doesn't always look for both local and global constraints

This is correct. Global searches can also be controlled separately by the global_sampling_ratio and global_constraint_search_after_n_seconds parameters. For example I set the sampling ratio to 0 on our robot to allow only localization in a local window around an initial pose that was sent by an external node.

So if I check the inter constraints on different trajectories I should be able to understand if it found a "localization match" between my current submap and the overall map?

If you have one active localization trajectory and match against a "frozen" trajectory (the map), the number of tag==inter_submap, trajectory==different constraints is the number of localization constraints that were found by local and/or global constraint search.

mapping_3d_local_trajectorybuilder{scores,cost,residuals} ... are these scores computed in a scan match between the current submap and the saved overall map on which I'm trying to localize or these scores are computed in the local SLAM procedure while actually building the submap?

Local trajectory builder is responsible for building submaps out of the sensor data via scan matching. The local/global constraint searches (loop closures, matching to the saved map) are handled separately.

[francescoricciuti]

Great, Thanks so much for your help!!

[francescoricciuti]

Hello everybody,

I'm reopening this issue because I need some more help on this.

I implemented some logics based on mapping_3d_pose_graph_constraints and on the ratio found/searched constraints from mapping_constraints_constraint_builder_3d_constraints. I'm able now to understand, with a really satisfying precision, when I localize after launching Cartographer node for the first time (thanks again @MichaelGrupp!) .

What I'm trying to do now is to understand when the precision of the localization decreases during my robot's task execution. The following picture is what I'm trying to identify and, possibly, quantify:

I know that I could tune my algorithm to avoid this, but still I want to be able to detect these events to increase the robustness of my system.

I've tried to detect these events using the metrics, but I failed.

I've tried the following thing:

• using mapping_3d_local_trajectory_builder_scores and mapping_3d_local_trajectory_builder_residuals to extract the current scan matching scores and comparing them with the average computed over the whole run. This strategy fails because these scores, from what I understood, are relative to the local SLAM scan matching, and then I get (relatively) low values everytime I change submap, while I get high values when I stay steady for a while, even if the overall localization quality is low

• using mapping_constraints_constraint_builder_3d_constraints to make statistics on the ratio found/searched constraints. It seems that on the long run I can kind of understand that something happened, but I couldn't detect small events like the one represented in the picture.

• using mapping_constraints_constraint_builder_3d_scores to compute the current local and global scores and using them both in comparison with the average computed over the whole run and considering the values by themselves. I don't get good results here and this is a bit disappointing and sursprising, because I would expect these values, the global one in particular, to be representative of the constraints quality and then of the overall localization. What I see instead is that there is almost no relation between these values and the events like the one represented in the picture, or at least I can't catch the relation.

On this last point I would like to ask some more help, beacuse I really can't get why I don't catch any significant info from these values, maybe I misunderstood something.

Any other suggestion is really appreciated!

Thanks, Francesco

[narogon]

Hello everybody,

I'm reopening this issue because I need some more help on this.

I implemented some logics based on mapping_3d_pose_graph_constraints and on the ratio found/searched constraints from mapping_constraints_constraint_builder_3d_constraints. I'm able now to understand, with a really satisfying precision, when I localize after launching Cartographer node for the first time (thanks again @MichaelGrupp!) .

What I'm trying to do now is to understand when the precision of the localization decreases during my robot's task execution. The following picture is what I'm trying to identify and, possibly, quantify:

I know that I could tune my algorithm to avoid this, but still I want to be able to detect these events to increase the robustness of my system.

I've tried to detect these events using the metrics, but I failed.

I've tried the following thing:

* using `mapping_3d_local_trajectory_builder_scores` and `mapping_3d_local_trajectory_builder_residuals` to extract the current scan matching scores and comparing them with the average computed over the whole run. This strategy fails because these scores, from what I understood, are relative to the local SLAM scan matching, and then I get (relatively) low values everytime I change submap, while I get high values when I stay steady for a while, even if the overall localization quality is low

* using `mapping_constraints_constraint_builder_3d_constraints` to make statistics on the ratio found/searched constraints. It seems that on the long run I can kind of understand that something happened, but I couldn't detect small events like the one represented in the picture.

* using `mapping_constraints_constraint_builder_3d_scores` to compute the current local and global scores and using them both in comparison with the average computed over the whole run and considering the values by themselves. I don't get good results here and this is a bit disappointing and sursprising, because I would expect these values, the global one in particular, to be representative of the constraints quality and then of the overall localization. What I see instead is that there is almost no relation between these values and the events like the one represented in the picture, or at least I can't catch the relation.

On this last point I would like to ask some more help, beacuse I really can't get why I don't catch any significant info from these values, maybe I misunderstood something.

Any other suggestion is really appreciated!

Thanks, Francesco

Hi everybody,

I'm working with an AGV in pure localization mode with a very good lidar. I need to know, like you, the quality of the position SLAM in order to stop the movements if it is lost or the map doesn't seems like the submap.

Sorry for my unknowledge but where you obtain the mapping_constraints_constraint_builder_3d_scores, mapping_constraints_constraint_builder_3d_constraints... values. I can't see any topic or log info where they are published.

In other way is it possible to check some parts of the map with more emphasis?, I mean I have some checkpoints with a characteristic form (a triangle with a rectangle at some distance ...) which position I know precisesly.

Thanks, Nacho

[narogon]

I think this is so related with #469 .

There is no answer in any of both issues. Is there anyway to check in pure localization mode how well the scan points match the loaded map?

[WilliamWoo45]

Hi everyone!

I have kind of similar questions with @narogon .

Is there anyway to measure in pure localization mode how well the laser scan points matched the loaded map? Which basically means, how to measure the pure localization (re-localization) quantitatively?

If the measurement can be achieved by obtaining the values of those constraints, like mapping_constraints_constraint_builder_{2d, 3d}_constraints, mapping_constraints_constraint_builder_{2d, 3d}_scores, and mapping_{2d, 3d}_pose_graph_constraints, then how can we obtain these values? I also didn't see any topic or log info where they are published.

Any suggestions or hints are appreciated. Thanks! @francescoricciuti @MichaelGrupp

[georgflick]

@NTUwilliam you need to add the argument -collect_metrics (check ROS-API) in your cartographer_ros launch file. Afterwards you can call the rosservice via rosservice call /cartographer/read_metrics to get these measurements.

[francescoricciuti]

If the measurement can be achieved by obtaining the values of those constraints, like mapping_constraints_constraintbuilder{2d, 3d}_constraints, mapping_constraints_constraintbuilder{2d, 3d}scores, and mapping{2d, 3d}_pose_graph_constraints

Yes, it can, or at least you can understand whether you are localized or not playing with those scores and you can build some sort of metrics on it

[WilliamWoo45]

If the measurement can be achieved by obtaining the values of those constraints, like mapping_constraints_constraintbuilder{2d, 3d}_constraints, mapping_constraints_constraintbuilder{2d, 3d}scores, and mapping{2d, 3d}_pose_graph_constraints

Yes, it can, or at least you can understand whether you are localized or not playing with those scores and you can build some sort of metrics on it

@francescoricciuti Thanks for your fast reply. But I'm wondering how can we obtain these values? I didn't see any topic or log info where they are published.

[WilliamWoo45]

@georgflick Thanks for your fast reply.

By saying:

add the argument -collect_metrics in your cartographer_ros launch file

Do you mean that adding the following code into the launch file ?demo_backpack_2d_localization.launch:

  <node name="cartographer_ros_msgs" pkg="cartographer_ros"
      type="cartographer_ros_msgs" args="
      -configuration_directory $(find cartographer_ros_msgs)/msg
      -collect_metrics
      " 
      output="screen">

BTW, I need to add the files like Metric.msg, MetricFamily.msg and MetricLabel.msg to the source directory of /src/cartographer_ros/cartographer_ros_msgs/msg (I think previous Cartographer ROS version doesn't have these files). Then I need to recompile (cmake) the whole workspace and the rosservice call /cartographer/read_metrics later can work, right?

[WilliamWoo45]

@MichaelGrupp @francescoricciuti @narogon @georgflick Hi all! Apart from the localization quality check issues, I'm still wondering the exact working principle of the pure localization mode, where three possible explanations (I assumed) are listed below for your reference:

1. In the Cartographer ROS Localization-only documentation, it says that "the localization-only mode of Cartographer which will run SLAM against the existing map and won’t build a new one". Does this mean that when runing the pure localization mode, actually the Cartographer is running a new SLAM process without updating the map (no mapping)?

2. When running the pure localization mode, is the submap matched with the pre-built overall map of the environment? If so, then maybe parameters like mapping_constraints_constraint_builder_{2d, 3d}_constraints and mapping_constraints_constraint_builder_{2d, 3d}_scores are used to judge whether the pure localization is successful? As Michael mentioned previously:

   Some things you can do:

   • mapping_constraints_constraint_builder_{2d, 3d}_constraints: e.g. calculate ratio of found vs. searched constraints (ideal: 1.0), over the whole runtime.
   • mapping_constraints_constraint_builder_{2d, 3d}_scores: this is a histogram of the matching scores that were found over the whole runtime. You can do statistics based on that.
   • mapping_{2d, 3d}_pose_graph_constraints: the current number of constraints in the pose graph. This metric is aware of trimming/deletion. If the number of tag==inter_submap, trajectory==different constraints is >0, you have found a "localization match". trajectory==same are loop closure constraints.

3. Do you have any idea what parameter is the global_localization_min_score? The Cartographer configuration file says it is the "Threshold below which global localizations are not trusted". However, may I understand this as a threshold to measure whether the pure localization is successful or not? Does this value set empirically by ourselves? An example value is set at global_localization_min_score = 0.6 in pose_graph.lua

What is your opinion? Any hint or advice is appreciated. Thanks!

Best Regards, William