Change submap state handling in 3D. (#327)
This follows the new code in 2D. Also adds a check that new constraints are only attempted for finished submaps.master
parent
f07f888dab
commit
1a376c07b2
|
@ -153,6 +153,8 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
|
|||
|
||||
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||
const mapping::SubmapId& submap_id) {
|
||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
|
||||
|
||||
// Only globally match against submaps not in this trajectory.
|
||||
if (node_id.trajectory_id != submap_id.trajectory_id &&
|
||||
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
|
||||
|
@ -260,10 +262,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
|||
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
||||
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
|
||||
CHECK(finished_submap_data.state == SubmapState::kActive);
|
||||
finished_submap_data.state = SubmapState::kFinished;
|
||||
// We have a new completed submap, so we look into adding constraints for
|
||||
// old scans.
|
||||
ComputeConstraintsForOldScans(finished_submap);
|
||||
finished_submap_data.state = SubmapState::kFinished;
|
||||
}
|
||||
constraint_builder_.NotifyEndOfScan();
|
||||
++num_scans_since_last_loop_closure_;
|
||||
|
|
|
@ -109,9 +109,9 @@ void SparsePoseGraph::AddScan(
|
|||
|
||||
if (submap_ids_.count(insertion_submaps.back()) == 0) {
|
||||
const mapping::SubmapId submap_id =
|
||||
submap_states_.Append(trajectory_id, SubmapState());
|
||||
submap_data_.Append(trajectory_id, SubmapData());
|
||||
submap_ids_.emplace(insertion_submaps.back(), submap_id);
|
||||
submap_states_.at(submap_id).submap = insertion_submaps.back();
|
||||
submap_data_.at(submap_id).submap = insertion_submaps.back();
|
||||
}
|
||||
const Submap* const finished_submap =
|
||||
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
|
||||
|
@ -149,6 +149,8 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
|
|||
|
||||
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||
const mapping::SubmapId& submap_id) {
|
||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
|
||||
|
||||
const transform::Rigid3d inverse_submap_pose =
|
||||
optimization_problem_.submap_data()
|
||||
.at(submap_id.trajectory_id)
|
||||
|
@ -163,7 +165,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
|||
|
||||
std::vector<mapping::TrajectoryNode> submap_nodes;
|
||||
for (const mapping::NodeId& submap_node_id :
|
||||
submap_states_.at(submap_id).node_ids) {
|
||||
submap_data_.at(submap_id).node_ids) {
|
||||
submap_nodes.push_back(mapping::TrajectoryNode{
|
||||
trajectory_nodes_.at(submap_node_id).constant_data,
|
||||
inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose});
|
||||
|
@ -185,7 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
|||
// yaw component will be handled by the matching procedure in the
|
||||
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
|
||||
constraint_builder_.MaybeAddGlobalConstraint(
|
||||
submap_id, submap_states_.at(submap_id).submap, node_id,
|
||||
submap_id, submap_data_.at(submap_id).submap, node_id,
|
||||
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
|
||||
submap_nodes, initial_relative_pose.rotation(),
|
||||
&trajectory_connectivity_);
|
||||
|
@ -198,7 +200,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
|||
if (node_id.trajectory_id == submap_id.trajectory_id ||
|
||||
scan_and_submap_trajectories_connected) {
|
||||
constraint_builder_.MaybeAddConstraint(
|
||||
submap_id, submap_states_.at(submap_id).submap, node_id,
|
||||
submap_id, submap_data_.at(submap_id).submap, node_id,
|
||||
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
|
||||
submap_nodes, initial_relative_pose);
|
||||
}
|
||||
|
@ -207,7 +209,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
|||
|
||||
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
||||
const auto submap_id = GetSubmapId(submap);
|
||||
const auto& submap_state = submap_states_.at(submap_id);
|
||||
const auto& submap_data = submap_data_.at(submap_id);
|
||||
|
||||
const auto& node_data = optimization_problem_.node_data();
|
||||
for (size_t trajectory_id = 0; trajectory_id != node_data.size();
|
||||
|
@ -216,7 +218,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
|||
++node_index) {
|
||||
const mapping::NodeId node_id{static_cast<int>(trajectory_id),
|
||||
static_cast<int>(node_index)};
|
||||
if (submap_state.node_ids.count(node_id) == 0) {
|
||||
if (submap_data.node_ids.count(node_id) == 0) {
|
||||
ComputeConstraint(node_id, submap_id);
|
||||
}
|
||||
}
|
||||
|
@ -248,8 +250,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
|||
scan_data->time, optimized_pose);
|
||||
for (const Submap* submap : insertion_submaps) {
|
||||
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
||||
CHECK(!submap_states_.at(submap_id).finished);
|
||||
submap_states_.at(submap_id).node_ids.emplace(node_id);
|
||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
|
||||
submap_data_.at(submap_id).node_ids.emplace(node_id);
|
||||
const transform::Rigid3d constraint_transform =
|
||||
submap->local_pose.inverse() * pose;
|
||||
constraints_.push_back(
|
||||
|
@ -260,14 +262,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
|||
Constraint::INTRA_SUBMAP});
|
||||
}
|
||||
|
||||
for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories();
|
||||
for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
|
||||
++trajectory_id) {
|
||||
for (int submap_index = 0;
|
||||
submap_index < submap_states_.num_indices(trajectory_id);
|
||||
submap_index < submap_data_.num_indices(trajectory_id);
|
||||
++submap_index) {
|
||||
const mapping::SubmapId submap_id{trajectory_id, submap_index};
|
||||
if (submap_states_.at(submap_id).finished) {
|
||||
CHECK_EQ(submap_states_.at(submap_id).node_ids.count(node_id), 0);
|
||||
if (submap_data_.at(submap_id).state == SubmapState::kFinished) {
|
||||
CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0);
|
||||
ComputeConstraint(node_id, submap_id);
|
||||
}
|
||||
}
|
||||
|
@ -275,12 +277,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
|||
|
||||
if (finished_submap != nullptr) {
|
||||
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
||||
SubmapState& finished_submap_state = submap_states_.at(finished_submap_id);
|
||||
CHECK(!finished_submap_state.finished);
|
||||
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
|
||||
CHECK(finished_submap_data.state == SubmapState::kActive);
|
||||
finished_submap_data.state = SubmapState::kFinished;
|
||||
// We have a new completed submap, so we look into adding constraints for
|
||||
// old scans.
|
||||
ComputeConstraintsForOldScans(finished_submap);
|
||||
finished_submap_state.finished = true;
|
||||
}
|
||||
constraint_builder_.NotifyEndOfScan();
|
||||
++num_scans_since_last_loop_closure_;
|
||||
|
@ -420,14 +422,14 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
|||
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
||||
const int trajectory_id) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
if (submap_states_.num_trajectories() <= trajectory_id ||
|
||||
submap_states_.num_indices(trajectory_id) == 0) {
|
||||
if (submap_data_.num_trajectories() <= trajectory_id ||
|
||||
submap_data_.num_indices(trajectory_id) == 0) {
|
||||
return transform::Rigid3d::Identity();
|
||||
}
|
||||
const auto extrapolated_submap_transforms =
|
||||
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
|
||||
return extrapolated_submap_transforms.back() *
|
||||
submap_states_
|
||||
submap_data_
|
||||
.at(mapping::SubmapId{
|
||||
trajectory_id,
|
||||
static_cast<int>(extrapolated_submap_transforms.size()) - 1})
|
||||
|
@ -450,17 +452,17 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
|||
const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
|
||||
submap_transforms,
|
||||
const int trajectory_id) const {
|
||||
if (trajectory_id >= submap_states_.num_trajectories()) {
|
||||
if (trajectory_id >= submap_data_.num_trajectories()) {
|
||||
return {transform::Rigid3d::Identity()};
|
||||
}
|
||||
|
||||
// Submaps for which we have optimized poses.
|
||||
std::vector<transform::Rigid3d> result;
|
||||
for (int submap_index = 0;
|
||||
submap_index != submap_states_.num_indices(trajectory_id);
|
||||
submap_index != submap_data_.num_indices(trajectory_id);
|
||||
++submap_index) {
|
||||
const mapping::SubmapId submap_id{trajectory_id, submap_index};
|
||||
const auto& state = submap_states_.at(submap_id);
|
||||
const auto& submap_data = submap_data_.at(submap_id);
|
||||
if (static_cast<size_t>(trajectory_id) < submap_transforms.size() &&
|
||||
result.size() < submap_transforms.at(trajectory_id).size()) {
|
||||
// Submaps for which we have optimized poses.
|
||||
|
@ -475,8 +477,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
|||
trajectory_id, static_cast<int>(result.size()) - 1};
|
||||
result.push_back(
|
||||
result.back() *
|
||||
submap_states_.at(previous_submap_id).submap->local_pose.inverse() *
|
||||
state.submap->local_pose);
|
||||
submap_data_.at(previous_submap_id).submap->local_pose.inverse() *
|
||||
submap_data.submap->local_pose);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -90,7 +90,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
||||
|
||||
private:
|
||||
struct SubmapState {
|
||||
// The current state of the submap in the background threads. When this
|
||||
// transitions to kFinished, all scans are tried to match against this submap.
|
||||
// Likewise, all new scans are matched against submaps which are finished.
|
||||
enum class SubmapState { kActive, kFinished, kTrimmed };
|
||||
struct SubmapData {
|
||||
const Submap* submap = nullptr;
|
||||
|
||||
// IDs of the scans that were inserted into this map together with
|
||||
|
@ -98,11 +102,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
// becomes 'finished'.
|
||||
std::set<mapping::NodeId> node_ids;
|
||||
|
||||
// Whether in the current state of the background thread this submap is
|
||||
// finished. When this transitions to true, all scans are tried to match
|
||||
// against this submap. Likewise, all new scans are matched against submaps
|
||||
// which are finished.
|
||||
bool finished = false;
|
||||
SubmapState state = SubmapState::kActive;
|
||||
};
|
||||
|
||||
// Handles a new work item.
|
||||
|
@ -182,7 +182,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
// before they take part in the background computations.
|
||||
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
||||
GUARDED_BY(mutex_);
|
||||
mapping::NestedVectorsById<SubmapState, mapping::SubmapId> submap_states_
|
||||
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
|
||||
GUARDED_BY(mutex_);
|
||||
|
||||
// Connectivity structure of our trajectories by IDs.
|
||||
|
|
Loading…
Reference in New Issue