Add option to perform scan matching at a minimum rate without needing to update map

[malban]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	NA
Primary OS tested on	Ubuntu 18.04
Robotic platform tested on	gazebo simulation of small indoor robot with single plane lidar

---

Description of contribution in a few bullet points

Add option to perform continuous scan matching to update the transform and pose even when the minimum travel distance or heading have not been reached for adding the scan into the map.

This is useful when a relatively high pose update rate is required for localization, but a dense graph is not desired.

Helpful when paired with #504 for integration with a downstream filter such as robot_localization.

Description of documentation updates required from your changes

Documentation updated

---

Future work that may be required in bullet points

NA

[SteveMacenski]

This adds nodes to the graph that are not removed if you only goal is to force every single iteration to scan match for odometry or something. If you want to process every single possible scan, simply do so by setting the minimum travel heading/distance to 0 in your configuration

[SteveMacenski]

Move this back to where it was originally, these were organized by use

[malban]

Move this back to where it was originally, these were organized by use

Will do. They seem related though, no? They are both used in the same place to determine if a new node should get added to the graph based on robot motion right? I might be missing something though.

This adds nodes to the graph that are not removed if you only goal is to force every single iteration to scan match for odometry or something. If you want to process every single possible scan, simply do so by setting the minimum travel heading/distance to 0 in your configuration

Setting minimum travel distance low resulted in a lot of nodes getting added to the graph and poor performance. I think adding a bunch of scans to the scan buffer that are really close to each other may have also decreased the quality of the map, resulting if faster drift.

Yeah, the goal is to have more or less every scan do a scan match, but not have every scan match result in an update to the graph or scan buffer.

[malban]

To be clear:

It seems like there is an advantage to having the graph and scan buffer be somewhat sparse, while at the same time returning scan matches at a high rate for odometry purposes.

A sparser graph results in longer run times before redundant nodes need to be removed. Why add a lot of redundant nodes in the first place?

For the scan buffer, I think the accumulation of error from the scan matches is slowed down by only adding new scans to the buffer after a minimum amount of distance, kind of like a key point. Otherwise, if each scan is scan matched against it's immediate predecessor, error is accumulated each scan instead of each keypoint.

[malban]

The use case is to be able to quickly compensate for things like wheel slip by having a fast scan matched pose, while also building up a relatively long lived map, which needs to be somewhat sparse for efficiency.

[SteveMacenski]

Will do. They seem related though, no? They are both used in the same place to determine if a new node should get added to the graph based on robot motion right? I might be missing something though.

There's a subtlety there - they're used internally to Karto but I also use one as a pre-check in the slam_toolbox code to help reduce compute. The separation reflects that.

Setting minimum travel distance low resulted in a lot of nodes getting added to the graph and poor performance. I think adding a bunch of scans to the scan buffer that are really close to each other may have also decreased the quality of the map, resulting if faster drift.

True, the rule of thumb in traditional scan matchers is that you want roughly 70% overlap, too much and things get jittery. I don't think this is a particularly good choice for odometry for that reason. This is a map->scan matcher (or realistically its doing N scans to scan matcher in localized area). For odometry purposes you'd probably be better off with a different odoemtry scan matcher that does relative changes in a buffer rather than a map graph.

[SteveMacenski]

I think that is a use-case for odometry - I don't think there's any need for registration to a map or globally consistent frame. If you just use the relative changes in the laser scan over a short buffer (or just the last scan) you can get that information with way less jitter. That's the standard kind of way for doing lidar odometry as well. This is still about relative motion to know that the wheels are slipping compared to what you should have theoretically be with a given set of controls over time.

[malban]

If you just use the relative changes in the laser scan over a short buffer (or just the last scan) you can get that information with way less jitter.

Is that not what is happening here?
https://github.com/malban/slam_toolbox/blob/continuous-scan-matching/lib/karto_sdk/src/Mapper.cpp#L2714-L2717

      m_pSequentialScanMatcher->MatchScan(pScan,
        m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()),
        bestPose,
        covariance);

That was my assumption, other than being in the map frame instead of an odom frame.

I understand actual loop closures that may occur when adding a scan to the graph may cause jumps that would not otherwise occur with just local relative registration.

Your point is taken about being able to address wheel slip separately, with only relative scan matching.

Regardless, it still seems useful to be able to have scan match results output at a rate that is not limited by the thresholds for adding scans to the map. Perhaps enable_continuous_matching is not the right parameter, but something like max_scan_match_interval, so if it's set to 1, would perform a scan match at least once a second even if the robot has not moved. Defaulting it to -1 would imply an infinite interval and 0 would imply matching all scans.

[malban]

If you just use the relative changes in the laser scan over a short buffer (or just the last scan) you can get that information with way less jitter.

In terms of jitter, are you meaning from loop closures or some other cause?

As mentioned above, I was under the assumption that m_pSequentialScanMatcher->MatchScan was doing essentially what you described.

Performing the additional scan matches against the sequential buffer won't modify the buffer or graph, so I don't see where the extra jitter would come from, aside from when the graph insertion criteria has been met and the graph and buffer are updated.

[SteveMacenski]

Not quite, the running scans are used to create a correlation grid, not direct scan-to-scan matching, which would provide actually much more useful/reliable information for incremental odometry.

I think that this is probably a technology mismatch.

[malban]

Ah, I see. Thanks for clarifying.

I'll look at options for doing scan-to-scan matching at the odom level.

I went ahead and updated the MR to use maximum_match_interval instead of enable_continuous_matching, which should give more flexibility on configuration.

Separate from the wheel speed slippage/odometry issue, I still think downstream consumers of the pose output are going to expect the data at some relatively steady rate, instead of just when the robot has moved enough.