Handle multiple frozen trajectories as one connected component. (#1610)

This fixes local constraint search in frozen maps built from multiple trajectories.

Local constraint search from a localization trajectory to a set of frozen trajectories
(i.e. the map) is done only among trajectories that are transitively connected in the
same connected component. If we set an initial pose, we create such a connection
to one frozen trajectory, but we were so far not able to do a local constraint search
w.r.t. to other frozen trajectories because they're not connected among each other.
Any constraints between them are not loaded (because they’re frozen).
master
Michael Grupp 2020-06-17 10:59:39 +02:00 committed by GitHub
parent e8c1d840e6
commit 82a3970f76
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 32 additions and 0 deletions

View File

@ -649,6 +649,22 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFrozen(trajectory_id));
// Connect multiple frozen trajectories among each other.
// This is required for localization against multiple frozen trajectories
// because we lose inter-trajectory constraints when freezing.
for (const auto& entry : data_.trajectories_state) {
const int other_trajectory_id = entry.first;
if (!IsTrajectoryFrozen(other_trajectory_id)) {
continue;
}
if (data_.trajectory_connectivity_state.TransitivelyConnected(
trajectory_id, other_trajectory_id)) {
// Already connected, nothing to do.
continue;
}
data_.trajectory_connectivity_state.Connect(
trajectory_id, other_trajectory_id, common::FromUniversal(0));
}
data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
return WorkItem::Result::kDoNotRunOptimization;
});

View File

@ -643,6 +643,22 @@ void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
CHECK(!IsTrajectoryFrozen(trajectory_id));
// Connect multiple frozen trajectories among each other.
// This is required for localization against multiple frozen trajectories
// because we lose inter-trajectory constraints when freezing.
for (const auto& entry : data_.trajectories_state) {
const int other_trajectory_id = entry.first;
if (!IsTrajectoryFrozen(other_trajectory_id)) {
continue;
}
if (data_.trajectory_connectivity_state.TransitivelyConnected(
trajectory_id, other_trajectory_id)) {
// Already connected, nothing to do.
continue;
}
data_.trajectory_connectivity_state.Connect(
trajectory_id, other_trajectory_id, common::FromUniversal(0));
}
data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
return WorkItem::Result::kDoNotRunOptimization;
});