Add localization quality estimation metric - #563
Conversation
Parameters for expected standard deviation x, y and yaw were added. Added compute_quality function. Signed-off-by: Patricio Palma <patricio.palma@creativa77.com.ar>
"localization_quality" publisher added. Parameter descriptors for expected stddev in x, y, and yaw have been added. Signed-off-by: Patricio Palma <patricio.palma@creativa77.com.ar>
c6cec0a to
6c839dc
Compare
Signed-off-by: Patricio Palma <patricio.palma@ekumenlabs.com>
Signed-off-by: Patricio Palma <patricio.palma@ekumenlabs.com>
| return estimate; | ||
| } | ||
|
|
||
| double Amcl::compute_quality(const Sophus::Matrix3d& actual_covariance) { |
There was a problem hiding this comment.
@papalmac meta, maybe followup: I think it may be worth to move this into the core, taking the nominal distribution, the motion model, and the last available estimate.
There was a problem hiding this comment.
This will also help unit testing this. That's step 1. Then we'll need to do some benchmarking and/or testing in the field. @agalbachicar would you be open to collaborate on that?
| for (int i = 0; i < 3; ++i) { | ||
| const double actual = actual_covariance.coeff(i, i); | ||
| if (actual > std::numeric_limits<double>::epsilon()) { | ||
| quality = std::min(quality, ref_covariance.coeff(i, i) / actual); |
There was a problem hiding this comment.
papalmac
changed the title
Generate reference states on Amcl class construction. Fix parameter value range. Signed-off-by: Patricio Palma <patricio.palma@ekumenlabs.com>
| std::mt19937 gen{42}; | ||
| ref_states_.reserve(params_.min_particles); | ||
| for (std::size_t i = 0; i < params_.min_particles; ++i) { | ||
| ref_states_.emplace_back(dist(gen)); | ||
| } |
There was a problem hiding this comment.
I'm not sure I would use min_particles here, especially since we are reusing systematically the same set, which means we will amplify any initial bias.
| for (int i = 0; i < 3; ++i) { | ||
| const double actual = actual_covariance.coeff(i, i); | ||
| if (actual > std::numeric_limits<double>::epsilon()) { | ||
| quality = std::min(quality, ref_covariance.coeff(i, i) / actual); |
There was a problem hiding this comment.
It's a bit distorsive than the quotient is made in terms of the squared values, since that will amplify any difference in the standard deviations.
| for (int i = 0; i < 3; ++i) { | ||
| const double actual = actual_covariance.coeff(i, i); | ||
| if (actual > std::numeric_limits<double>::epsilon()) { | ||
| quality = std::min(quality, ref_covariance.coeff(i, i) / actual); |
There was a problem hiding this comment.
having a single index for all three dimensions clouds poor choices for a particular dimension. I think all three should be outputs.
There was a problem hiding this comment.
Also, calculating the min over the three with no averaging is very biasing, since you only need one of the values to take a tail end value of the distribution to make a huge difference in the results.
… yaw. Signed-off-by: Patricio Palma <patricio.palma@ekumenlabs.com>
Documentation for input parameters now states "typical stddev" for x, y and yaw. Signed-off-by: Patricio Palma <patricio.palma@ekumenlabs.com>
3 participants
Proposed changes
Implements a quality/degradation metric based in a comparison between a expected covariance and the real covariance. It is based in the answer in #562.
A new publisher "localization_quality" provides a float between [0, 1] , the closer to 1 better quality.
Type of change
Checklist
Put an
xin the boxes that apply. This is simply a reminder of what we will require before merging your code.Additional comments
It is my first PR. Need some help to test this and to add a test to check the feature.