Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
malban wants to merge 4 commits into
base: galactic
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8d6a35f
Add option to perform continuous scan matching to update the transfor…
malban Jun 21, 2022
719eb98
Tweaks to parameter documentation.
malban Jun 22, 2022
381bf28
Merge branch 'galactic' into continuous-scan-matching
malban Jun 22, 2022
dca9c10
Refactor scan match only logic to be based on a time interval.
malban Jun 22, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 5 additions & 3 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -248,16 +248,18 @@ The following settings and options are exposed to you. My default configuration

`stack_size_to_use` - The number of bytes to reset the stack size to, to enable serialization/deserialization of files. A liberal default is 40000000, but less is fine.

`minimum_travel_distance` - Minimum distance of travel before processing a new scan
`minimum_travel_distance` - Minimum distance of travel before processing a new scan into the map

`minimum_travel_heading` - Minimum changing in heading before processing a new scan into the map
Copy link
Owner

@SteveMacenski SteveMacenski Jun 21, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@malban malban Jun 21, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

@malban malban Jun 21, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

@malban malban Jun 21, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Owner

@SteveMacenski SteveMacenski Jun 22, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Owner

@SteveMacenski SteveMacenski Jun 22, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

@malban malban Jun 22, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

@malban malban Jun 22, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Owner

@SteveMacenski SteveMacenski Jun 22, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

@malban malban Jun 22, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


`enable_continuous_matching` - Perform 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

## Matcher Params

`use_scan_matching` - whether to use scan matching to refine odometric pose (uh, why would you not?)

`use_scan_barycenter` - Whether to use the barycenter or scan pose

`minimum_travel_heading` - Minimum changing in heading to justify an update

`scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode

`scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer
Expand Down
1 change: 1 addition & 0 deletions include/slam_toolbox/slam_toolbox_common.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,6 +143,7 @@ class SlamToolbox : public rclcpp::Node

double resolution_;
bool first_measurement_, enable_interactive_mode_;
bool enable_continuous_matching_;

// Book keeping
std::unique_ptr<mapper_utils::SMapper> smapper_;
Expand Down
5 changes: 4 additions & 1 deletion lib/karto_sdk/include/karto_sdk/Mapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1994,9 +1994,12 @@ class KARTO_EXPORT Mapper : public Module
* is that of the range device originating the scan. Note that the mapper will set corrected pose
* information in the scan object.
*
* @param force_match_only Flag to force a scan match regardless of distance or angle traveled and to only return
* the resulting pose instead of adding the scan to the map.
*
* @return true if the scan was added successfully, false otherwise
*/
virtual kt_bool Process(LocalizedRangeScan * pScan);
virtual kt_bool Process(LocalizedRangeScan * pScan, bool force_match_only);

/**
* Process an Object
Expand Down
34 changes: 18 additions & 16 deletions lib/karto_sdk/src/Mapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2676,7 +2676,7 @@ kt_bool Mapper::Process(Object * /*pObject*/) // NOLINT
return true;
}

kt_bool Mapper::Process(LocalizedRangeScan * pScan)
kt_bool Mapper::Process(LocalizedRangeScan * pScan, bool force_match_only)
{
if (pScan != NULL) {
karto::LaserRangeFinder * pLaserRangeFinder = pScan->GetLaserRangeFinder();
Expand All @@ -2701,7 +2701,7 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan)
}

// test if scan is outside minimum boundary or if heading is larger then minimum heading
if (!HasMovedEnough(pScan, pLastScan)) {
if (!force_match_only && !HasMovedEnough(pScan, pLastScan)) {
return false;
}

Expand All @@ -2718,26 +2718,28 @@ kt_bool Mapper::Process(LocalizedRangeScan * pScan)
pScan->SetSensorPose(bestPose);
}

// add scan to buffer and assign id
m_pMapperSensorManager->AddScan(pScan);
if (!force_match_only) {
// add scan to buffer and assign id
m_pMapperSensorManager->AddScan(pScan);

if (m_pUseScanMatching->GetValue()) {
// add to graph
m_pGraph->AddVertex(pScan);
m_pGraph->AddEdges(pScan, covariance);
if (m_pUseScanMatching->GetValue()) {
// add to graph
m_pGraph->AddVertex(pScan);
m_pGraph->AddEdges(pScan, covariance);

m_pMapperSensorManager->AddRunningScan(pScan);
m_pMapperSensorManager->AddRunningScan(pScan);

if (m_pDoLoopClosing->GetValue()) {
std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
const_forEach(std::vector<Name>, &deviceNames)
{
m_pGraph->TryCloseLoop(pScan, *iter);
if (m_pDoLoopClosing->GetValue()) {
std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
const_forEach(std::vector<Name>, &deviceNames)
{
m_pGraph->TryCloseLoop(pScan, *iter);
}
}
}
}

m_pMapperSensorManager->SetLastScan(pScan);
m_pMapperSensorManager->SetLastScan(pScan);
}

return true;
}
Expand Down
13 changes: 12 additions & 1 deletion src/slam_toolbox_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,9 @@ void SlamToolbox::setParams()
enable_interactive_mode_ = this->declare_parameter("enable_interactive_mode",
enable_interactive_mode_);

enable_continuous_matching_ = false;
enable_continuous_matching_ = this->declare_parameter("enable_continuous_matching", enable_continuous_matching_);

double tmp_val = 0.5;
tmp_val = this->declare_parameter("transform_timeout", tmp_val);
transform_timeout_ = rclcpp::Duration::from_seconds(tmp_val);
Expand Down Expand Up @@ -542,9 +545,17 @@ LocalizedRangeScan * SlamToolbox::addScan(
// Add the localized range scan to the smapper
boost::mutex::scoped_lock lock(smapper_mutex_);
bool processed = false, update_reprocessing_transform = false;
bool match_only = false;

if (processor_type_ == PROCESS) {
processed = smapper_->getMapper()->Process(range_scan);

// if continuos mapping is enabled, force a scan match without adding it to
// the graph or scan buffer
if (!processed && enable_continuous_matching_) {
match_only = true;
processed = smapper_->getMapper()->Process(range_scan, true);
}
} else if (processor_type_ == PROCESS_FIRST_NODE) {
processed = smapper_->getMapper()->ProcessAtDock(range_scan);
processor_type_ = PROCESS;
Expand All @@ -571,7 +582,7 @@ LocalizedRangeScan * SlamToolbox::addScan(
// if successfully processed, create odom to map transformation
// and add our scan to storage
if (processed) {
if (enable_interactive_mode_) {
if (enable_interactive_mode_ && !match_only) {
scan_holder_->addScan(*scan);
}

Expand Down