parent
fde9272533
commit
c084624958
|
@ -115,15 +115,16 @@ void SparsePoseGraph::AddScan(
|
||||||
});
|
});
|
||||||
trajectory_connectivity_.Add(trajectory_id);
|
trajectory_connectivity_.Add(trajectory_id);
|
||||||
|
|
||||||
if (submap_indices_.count(insertion_submaps.back()) == 0) {
|
if (submap_ids_.count(insertion_submaps.back()) == 0) {
|
||||||
submap_indices_.emplace(insertion_submaps.back(),
|
submap_states_.resize(
|
||||||
static_cast<int>(submap_indices_.size()));
|
std::max<size_t>(submap_states_.size(), trajectory_id + 1));
|
||||||
submap_states_.emplace_back();
|
auto& trajectory_submap_states = submap_states_.at(trajectory_id);
|
||||||
submap_states_.back().submap = insertion_submaps.back();
|
submap_ids_.emplace(
|
||||||
submap_states_.back().id = mapping::SubmapId{
|
insertion_submaps.back(),
|
||||||
trajectory_id, num_submaps_in_trajectory_[trajectory_id]};
|
mapping::SubmapId{trajectory_id,
|
||||||
++num_submaps_in_trajectory_[trajectory_id];
|
static_cast<int>(trajectory_submap_states.size())});
|
||||||
CHECK_EQ(submap_states_.size(), submap_indices_.size());
|
trajectory_submap_states.emplace_back();
|
||||||
|
trajectory_submap_states.back().submap = insertion_submaps.back();
|
||||||
}
|
}
|
||||||
const mapping::Submap* const finished_submap =
|
const mapping::Submap* const finished_submap =
|
||||||
insertion_submaps.front()->finished_probability_grid != nullptr
|
insertion_submaps.front()->finished_probability_grid != nullptr
|
||||||
|
@ -138,8 +139,9 @@ void SparsePoseGraph::AddScan(
|
||||||
}
|
}
|
||||||
|
|
||||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||||
ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps,
|
ComputeConstraintsForScan(flat_scan_index, matching_submap,
|
||||||
finished_submap, pose, covariance);
|
insertion_submaps, finished_submap, pose,
|
||||||
|
covariance);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -164,8 +166,7 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
|
||||||
}
|
}
|
||||||
|
|
||||||
void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
||||||
const int submap_index) {
|
const mapping::SubmapId& submap_id) {
|
||||||
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
|
|
||||||
const transform::Rigid2d relative_pose =
|
const transform::Rigid2d relative_pose =
|
||||||
optimization_problem_.submap_data()
|
optimization_problem_.submap_data()
|
||||||
.at(submap_id.trajectory_id)
|
.at(submap_id.trajectory_id)
|
||||||
|
@ -176,27 +177,31 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
||||||
const mapping::Submaps* const scan_trajectory =
|
const mapping::Submaps* const scan_trajectory =
|
||||||
trajectory_nodes_[scan_index].constant_data->trajectory;
|
trajectory_nodes_[scan_index].constant_data->trajectory;
|
||||||
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
|
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
|
||||||
const int submap_trajectory_id =
|
|
||||||
submap_states_[submap_index].id.trajectory_id;
|
|
||||||
|
|
||||||
// Only globally match against submaps not in this trajectory.
|
// Only globally match against submaps not in this trajectory.
|
||||||
if (scan_trajectory_id != submap_trajectory_id &&
|
if (scan_trajectory_id != submap_id.trajectory_id &&
|
||||||
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
|
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
|
||||||
constraint_builder_.MaybeAddGlobalConstraint(
|
constraint_builder_.MaybeAddGlobalConstraint(
|
||||||
submap_id, submap_states_[submap_index].submap,
|
submap_id,
|
||||||
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.submap,
|
||||||
scan_index_to_node_id_.at(scan_index), scan_index,
|
scan_index_to_node_id_.at(scan_index), scan_index,
|
||||||
&trajectory_connectivity_,
|
&trajectory_connectivity_,
|
||||||
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
|
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
|
||||||
} else {
|
} else {
|
||||||
const bool scan_and_submap_trajectories_connected =
|
const bool scan_and_submap_trajectories_connected =
|
||||||
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
|
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
|
||||||
reverse_connected_components_.count(submap_trajectory_id) > 0 &&
|
reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
|
||||||
reverse_connected_components_.at(scan_trajectory_id) ==
|
reverse_connected_components_.at(scan_trajectory_id) ==
|
||||||
reverse_connected_components_.at(submap_trajectory_id);
|
reverse_connected_components_.at(submap_id.trajectory_id);
|
||||||
if (scan_trajectory_id == submap_trajectory_id ||
|
if (scan_trajectory_id == submap_id.trajectory_id ||
|
||||||
scan_and_submap_trajectories_connected) {
|
scan_and_submap_trajectories_connected) {
|
||||||
constraint_builder_.MaybeAddConstraint(
|
constraint_builder_.MaybeAddConstraint(
|
||||||
submap_id, submap_states_[submap_index].submap,
|
submap_id,
|
||||||
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.submap,
|
||||||
scan_index_to_node_id_.at(scan_index), scan_index,
|
scan_index_to_node_id_.at(scan_index), scan_index,
|
||||||
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
|
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
|
||||||
relative_pose);
|
relative_pose);
|
||||||
|
@ -206,14 +211,16 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
||||||
|
|
||||||
void SparsePoseGraph::ComputeConstraintsForOldScans(
|
void SparsePoseGraph::ComputeConstraintsForOldScans(
|
||||||
const mapping::Submap* submap) {
|
const mapping::Submap* submap) {
|
||||||
const int submap_index = GetSubmapIndex(submap);
|
const auto submap_id = GetSubmapId(submap);
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
CHECK_GT(node_data.size(), 0);
|
CHECK_GT(node_data.size(), 0);
|
||||||
CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
|
CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
|
||||||
const int num_nodes = node_data.size();
|
const int num_nodes = node_data.size();
|
||||||
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
||||||
if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
|
if (submap_states_.at(submap_id.trajectory_id)
|
||||||
ComputeConstraint(scan_index, submap_index);
|
.at(submap_id.submap_index)
|
||||||
|
.scan_indices.count(scan_index) == 0) {
|
||||||
|
ComputeConstraint(scan_index, submap_id);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -238,16 +245,20 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose,
|
trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose,
|
||||||
optimized_pose);
|
optimized_pose);
|
||||||
for (const mapping::Submap* submap : insertion_submaps) {
|
for (const mapping::Submap* submap : insertion_submaps) {
|
||||||
const int submap_index = GetSubmapIndex(submap);
|
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
||||||
CHECK(!submap_states_[submap_index].finished);
|
CHECK(!submap_states_.at(submap_id.trajectory_id)
|
||||||
submap_states_[submap_index].scan_indices.emplace(scan_index);
|
.at(submap_id.submap_index)
|
||||||
|
.finished);
|
||||||
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.scan_indices.emplace(scan_index);
|
||||||
// Unchanged covariance as (submap <- map) is a translation.
|
// Unchanged covariance as (submap <- map) is a translation.
|
||||||
const transform::Rigid2d constraint_transform =
|
const transform::Rigid2d constraint_transform =
|
||||||
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
|
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
|
||||||
constexpr double kFakePositionCovariance = 1e-6;
|
constexpr double kFakePositionCovariance = 1e-6;
|
||||||
constexpr double kFakeOrientationCovariance = 1e-6;
|
constexpr double kFakeOrientationCovariance = 1e-6;
|
||||||
constraints_.push_back(Constraint{
|
constraints_.push_back(Constraint{
|
||||||
submap_states_[submap_index].id,
|
submap_id,
|
||||||
scan_index_to_node_id_.at(scan_index),
|
scan_index_to_node_id_.at(scan_index),
|
||||||
{transform::Embed3D(constraint_transform),
|
{transform::Embed3D(constraint_transform),
|
||||||
common::ComputeSpdMatrixSqrtInverse(
|
common::ComputeSpdMatrixSqrtInverse(
|
||||||
|
@ -258,18 +269,28 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
Constraint::INTRA_SUBMAP});
|
Constraint::INTRA_SUBMAP});
|
||||||
}
|
}
|
||||||
|
|
||||||
CHECK_LT(submap_states_.size(), std::numeric_limits<int>::max());
|
for (size_t trajectory_id = 0; trajectory_id < submap_states_.size();
|
||||||
const int num_submaps = submap_states_.size();
|
++trajectory_id) {
|
||||||
for (int submap_index = 0; submap_index != num_submaps; ++submap_index) {
|
for (size_t submap_index = 0;
|
||||||
if (submap_states_[submap_index].finished) {
|
submap_index < submap_states_.at(trajectory_id).size();
|
||||||
CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0);
|
++submap_index) {
|
||||||
ComputeConstraint(scan_index, submap_index);
|
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
||||||
|
CHECK_EQ(submap_states_.at(trajectory_id)
|
||||||
|
.at(submap_index)
|
||||||
|
.scan_indices.count(scan_index),
|
||||||
|
0);
|
||||||
|
ComputeConstraint(scan_index,
|
||||||
|
mapping::SubmapId{static_cast<int>(trajectory_id),
|
||||||
|
static_cast<int>(submap_index)});
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (finished_submap != nullptr) {
|
if (finished_submap != nullptr) {
|
||||||
const int finished_submap_index = GetSubmapIndex(finished_submap);
|
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
||||||
SubmapState& finished_submap_state = submap_states_[finished_submap_index];
|
SubmapState& finished_submap_state =
|
||||||
|
submap_states_.at(finished_submap_id.trajectory_id)
|
||||||
|
.at(finished_submap_id.submap_index);
|
||||||
CHECK(!finished_submap_state.finished);
|
CHECK(!finished_submap_state.finished);
|
||||||
// We have a new completed submap, so we look into adding constraints for
|
// We have a new completed submap, so we look into adding constraints for
|
||||||
// old scans.
|
// old scans.
|
||||||
|
@ -441,44 +462,33 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
||||||
if (trajectory_ids_.count(trajectory) == 0) {
|
if (trajectory_ids_.count(trajectory) == 0) {
|
||||||
return {transform::Rigid3d::Identity()};
|
return {transform::Rigid3d::Identity()};
|
||||||
}
|
}
|
||||||
const int trajectory_id = trajectory_ids_.at(trajectory);
|
const size_t trajectory_id = trajectory_ids_.at(trajectory);
|
||||||
size_t flat_index = 0;
|
if (trajectory_id >= submap_states_.size()) {
|
||||||
size_t flat_index_of_result_back = -1;
|
return {transform::Rigid3d::Identity()};
|
||||||
|
}
|
||||||
|
|
||||||
// Submaps for which we have optimized poses.
|
// Submaps for which we have optimized poses.
|
||||||
std::vector<transform::Rigid3d> result;
|
std::vector<transform::Rigid3d> result;
|
||||||
for (; flat_index != submap_states_.size(); ++flat_index) {
|
for (const auto& state : submap_states_.at(trajectory_id)) {
|
||||||
const auto& state = submap_states_[flat_index];
|
if (trajectory_id < submap_transforms.size() &&
|
||||||
if (state.id.trajectory_id != trajectory_id) {
|
result.size() < submap_transforms.at(trajectory_id).size()) {
|
||||||
continue;
|
// Submaps for which we have optimized poses.
|
||||||
}
|
result.push_back(
|
||||||
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
|
Embed3D(submap_transforms.at(trajectory_id).at(result.size()).pose));
|
||||||
result.size() >= submap_transforms.at(trajectory_id).size()) {
|
} else if (result.empty()) {
|
||||||
break;
|
|
||||||
}
|
|
||||||
result.push_back(transform::Embed3D(
|
|
||||||
submap_transforms.at(trajectory_id).at(result.size()).pose));
|
|
||||||
flat_index_of_result_back = flat_index;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Extrapolate to the remaining submaps.
|
|
||||||
for (; flat_index != submap_states_.size(); ++flat_index) {
|
|
||||||
const auto& state = submap_states_[flat_index];
|
|
||||||
if (state.id.trajectory_id != trajectory_id) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
if (result.empty()) {
|
|
||||||
result.push_back(transform::Rigid3d::Identity());
|
result.push_back(transform::Rigid3d::Identity());
|
||||||
} else {
|
} else {
|
||||||
// Accessing local_pose() in Submaps is okay, since the member is const.
|
// Extrapolate to the remaining submaps. Accessing local_pose() in
|
||||||
|
// Submaps is okay, since the member is const.
|
||||||
result.push_back(result.back() *
|
result.push_back(result.back() *
|
||||||
submap_states_[flat_index_of_result_back]
|
submap_states_.at(trajectory_id)
|
||||||
|
.at(result.size() - 1)
|
||||||
.submap->local_pose()
|
.submap->local_pose()
|
||||||
.inverse() *
|
.inverse() *
|
||||||
state.submap->local_pose());
|
state.submap->local_pose());
|
||||||
}
|
}
|
||||||
flat_index_of_result_back = flat_index;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (result.empty()) {
|
if (result.empty()) {
|
||||||
result.push_back(transform::Rigid3d::Identity());
|
result.push_back(transform::Rigid3d::Identity());
|
||||||
}
|
}
|
||||||
|
|
|
@ -108,22 +108,16 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// against this submap. Likewise, all new scans are matched against submaps
|
// against this submap. Likewise, all new scans are matched against submaps
|
||||||
// which are finished.
|
// which are finished.
|
||||||
bool finished = false;
|
bool finished = false;
|
||||||
|
|
||||||
mapping::SubmapId id;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
// Handles a new work item.
|
// Handles a new work item.
|
||||||
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
|
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
|
||||||
|
|
||||||
int GetSubmapIndex(const mapping::Submap* submap) const REQUIRES(mutex_) {
|
|
||||||
const auto iterator = submap_indices_.find(submap);
|
|
||||||
CHECK(iterator != submap_indices_.end());
|
|
||||||
return iterator->second;
|
|
||||||
}
|
|
||||||
|
|
||||||
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
|
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
|
||||||
REQUIRES(mutex_) {
|
REQUIRES(mutex_) {
|
||||||
return submap_states_.at(GetSubmapIndex(submap)).id;
|
const auto iterator = submap_ids_.find(submap);
|
||||||
|
CHECK(iterator != submap_ids_.end());
|
||||||
|
return iterator->second;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Grows the optimization problem to have an entry for every element of
|
// Grows the optimization problem to have an entry for every element of
|
||||||
|
@ -140,8 +134,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
|
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
|
||||||
|
|
||||||
// Computes constraints for a scan and submap pair.
|
// Computes constraints for a scan and submap pair.
|
||||||
void ComputeConstraint(const int scan_index, const int submap_index)
|
void ComputeConstraint(const int scan_index,
|
||||||
REQUIRES(mutex_);
|
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
|
||||||
|
|
||||||
// Adds constraints for older scans whenever a new submap is finished.
|
// Adds constraints for older scans whenever a new submap is finished.
|
||||||
void ComputeConstraintsForOldScans(const mapping::Submap* submap)
|
void ComputeConstraintsForOldScans(const mapping::Submap* submap)
|
||||||
|
@ -191,11 +185,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
|
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
|
||||||
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
|
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Submaps get assigned an index and state as soon as they are seen, even
|
// Submaps get assigned an ID and state as soon as they are seen, even
|
||||||
// before they take part in the background computations.
|
// before they take part in the background computations.
|
||||||
std::map<const mapping::Submap*, int> submap_indices_ GUARDED_BY(mutex_);
|
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
||||||
std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
std::map<int, int> num_submaps_in_trajectory_ GUARDED_BY(mutex_);
|
std::vector<std::vector<SubmapState>> submap_states_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Mapping to flat indices to aid the transition to per-trajectory data
|
// Mapping to flat indices to aid the transition to per-trajectory data
|
||||||
// structures.
|
// structures.
|
||||||
|
|
|
@ -112,15 +112,16 @@ void SparsePoseGraph::AddScan(
|
||||||
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
|
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
|
||||||
trajectory_connectivity_.Add(trajectory_id);
|
trajectory_connectivity_.Add(trajectory_id);
|
||||||
|
|
||||||
if (submap_indices_.count(insertion_submaps.back()) == 0) {
|
if (submap_ids_.count(insertion_submaps.back()) == 0) {
|
||||||
submap_indices_.emplace(insertion_submaps.back(),
|
submap_states_.resize(
|
||||||
static_cast<int>(submap_indices_.size()));
|
std::max<size_t>(submap_states_.size(), trajectory_id + 1));
|
||||||
submap_states_.emplace_back();
|
auto& trajectory_submap_states = submap_states_.at(trajectory_id);
|
||||||
submap_states_.back().submap = insertion_submaps.back();
|
submap_ids_.emplace(
|
||||||
submap_states_.back().id = mapping::SubmapId{
|
insertion_submaps.back(),
|
||||||
trajectory_id, num_submaps_in_trajectory_[trajectory_id]};
|
mapping::SubmapId{trajectory_id,
|
||||||
++num_submaps_in_trajectory_[trajectory_id];
|
static_cast<int>(trajectory_submap_states.size())});
|
||||||
CHECK_EQ(submap_states_.size(), submap_indices_.size());
|
trajectory_submap_states.emplace_back();
|
||||||
|
trajectory_submap_states.back().submap = insertion_submaps.back();
|
||||||
}
|
}
|
||||||
const Submap* const finished_submap =
|
const Submap* const finished_submap =
|
||||||
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
|
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
|
||||||
|
@ -133,8 +134,9 @@ void SparsePoseGraph::AddScan(
|
||||||
}
|
}
|
||||||
|
|
||||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||||
ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps,
|
ComputeConstraintsForScan(flat_scan_index, matching_submap,
|
||||||
finished_submap, pose, covariance);
|
insertion_submaps, finished_submap, pose,
|
||||||
|
covariance);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -164,8 +166,7 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
|
||||||
}
|
}
|
||||||
|
|
||||||
void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
||||||
const int submap_index) {
|
const mapping::SubmapId& submap_id) {
|
||||||
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
|
|
||||||
const transform::Rigid3d relative_pose =
|
const transform::Rigid3d relative_pose =
|
||||||
optimization_problem_.submap_data()
|
optimization_problem_.submap_data()
|
||||||
.at(submap_id.trajectory_id)
|
.at(submap_id.trajectory_id)
|
||||||
|
@ -176,26 +177,30 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
||||||
const mapping::Submaps* const scan_trajectory =
|
const mapping::Submaps* const scan_trajectory =
|
||||||
trajectory_nodes_[scan_index].constant_data->trajectory;
|
trajectory_nodes_[scan_index].constant_data->trajectory;
|
||||||
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
|
const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
|
||||||
const int submap_trajectory_id =
|
|
||||||
submap_states_[submap_index].id.trajectory_id;
|
|
||||||
|
|
||||||
// Only globally match against submaps not in this trajectory.
|
// Only globally match against submaps not in this trajectory.
|
||||||
if (scan_trajectory_id != submap_trajectory_id &&
|
if (scan_trajectory_id != submap_id.trajectory_id &&
|
||||||
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
|
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
|
||||||
constraint_builder_.MaybeAddGlobalConstraint(
|
constraint_builder_.MaybeAddGlobalConstraint(
|
||||||
submap_id, submap_states_[submap_index].submap,
|
submap_id,
|
||||||
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.submap,
|
||||||
scan_index_to_node_id_.at(scan_index), scan_index,
|
scan_index_to_node_id_.at(scan_index), scan_index,
|
||||||
&trajectory_connectivity_, trajectory_nodes_);
|
&trajectory_connectivity_, trajectory_nodes_);
|
||||||
} else {
|
} else {
|
||||||
const bool scan_and_submap_trajectories_connected =
|
const bool scan_and_submap_trajectories_connected =
|
||||||
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
|
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
|
||||||
reverse_connected_components_.count(submap_trajectory_id) > 0 &&
|
reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
|
||||||
reverse_connected_components_.at(scan_trajectory_id) ==
|
reverse_connected_components_.at(scan_trajectory_id) ==
|
||||||
reverse_connected_components_.at(submap_trajectory_id);
|
reverse_connected_components_.at(submap_id.trajectory_id);
|
||||||
if (scan_trajectory_id == submap_trajectory_id ||
|
if (scan_trajectory_id == submap_id.trajectory_id ||
|
||||||
scan_and_submap_trajectories_connected) {
|
scan_and_submap_trajectories_connected) {
|
||||||
constraint_builder_.MaybeAddConstraint(
|
constraint_builder_.MaybeAddConstraint(
|
||||||
submap_id, submap_states_[submap_index].submap,
|
submap_id,
|
||||||
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.submap,
|
||||||
scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_,
|
scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_,
|
||||||
relative_pose);
|
relative_pose);
|
||||||
}
|
}
|
||||||
|
@ -203,14 +208,16 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
|
||||||
}
|
}
|
||||||
|
|
||||||
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
||||||
const int submap_index = GetSubmapIndex(submap);
|
const auto submap_id = GetSubmapId(submap);
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
CHECK_GT(node_data.size(), 0);
|
CHECK_GT(node_data.size(), 0);
|
||||||
CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
|
CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
|
||||||
const int num_nodes = node_data.size();
|
const int num_nodes = node_data.size();
|
||||||
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
||||||
if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
|
if (submap_states_.at(submap_id.trajectory_id)
|
||||||
ComputeConstraint(scan_index, submap_index);
|
.at(submap_id.submap_index)
|
||||||
|
.scan_indices.count(scan_index) == 0) {
|
||||||
|
ComputeConstraint(scan_index, submap_id);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -235,14 +242,18 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
trajectory_ids_.at(scan_data->trajectory), scan_data->time,
|
trajectory_ids_.at(scan_data->trajectory), scan_data->time,
|
||||||
optimized_pose);
|
optimized_pose);
|
||||||
for (const Submap* submap : insertion_submaps) {
|
for (const Submap* submap : insertion_submaps) {
|
||||||
const int submap_index = GetSubmapIndex(submap);
|
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
||||||
CHECK(!submap_states_[submap_index].finished);
|
CHECK(!submap_states_.at(submap_id.trajectory_id)
|
||||||
submap_states_[submap_index].scan_indices.emplace(scan_index);
|
.at(submap_id.submap_index)
|
||||||
|
.finished);
|
||||||
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.scan_indices.emplace(scan_index);
|
||||||
// Unchanged covariance as (submap <- map) is a translation.
|
// Unchanged covariance as (submap <- map) is a translation.
|
||||||
const transform::Rigid3d constraint_transform =
|
const transform::Rigid3d constraint_transform =
|
||||||
submap->local_pose().inverse() * pose;
|
submap->local_pose().inverse() * pose;
|
||||||
constraints_.push_back(
|
constraints_.push_back(
|
||||||
Constraint{submap_states_[submap_index].id,
|
Constraint{submap_id,
|
||||||
scan_index_to_node_id_.at(scan_index),
|
scan_index_to_node_id_.at(scan_index),
|
||||||
{constraint_transform,
|
{constraint_transform,
|
||||||
common::ComputeSpdMatrixSqrtInverse(
|
common::ComputeSpdMatrixSqrtInverse(
|
||||||
|
@ -251,18 +262,28 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
Constraint::INTRA_SUBMAP});
|
Constraint::INTRA_SUBMAP});
|
||||||
}
|
}
|
||||||
|
|
||||||
CHECK_LT(submap_states_.size(), std::numeric_limits<int>::max());
|
for (size_t trajectory_id = 0; trajectory_id < submap_states_.size();
|
||||||
const int num_submaps = submap_states_.size();
|
++trajectory_id) {
|
||||||
for (int submap_index = 0; submap_index != num_submaps; ++submap_index) {
|
for (size_t submap_index = 0;
|
||||||
if (submap_states_[submap_index].finished) {
|
submap_index < submap_states_.at(trajectory_id).size();
|
||||||
CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0);
|
++submap_index) {
|
||||||
ComputeConstraint(scan_index, submap_index);
|
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
||||||
|
CHECK_EQ(submap_states_.at(trajectory_id)
|
||||||
|
.at(submap_index)
|
||||||
|
.scan_indices.count(scan_index),
|
||||||
|
0);
|
||||||
|
ComputeConstraint(scan_index,
|
||||||
|
mapping::SubmapId{static_cast<int>(trajectory_id),
|
||||||
|
static_cast<int>(submap_index)});
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (finished_submap != nullptr) {
|
if (finished_submap != nullptr) {
|
||||||
const int finished_submap_index = GetSubmapIndex(finished_submap);
|
const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap);
|
||||||
SubmapState& finished_submap_state = submap_states_[finished_submap_index];
|
SubmapState& finished_submap_state =
|
||||||
|
submap_states_.at(finished_submap_id.trajectory_id)
|
||||||
|
.at(finished_submap_id.submap_index);
|
||||||
CHECK(!finished_submap_state.finished);
|
CHECK(!finished_submap_state.finished);
|
||||||
// We have a new completed submap, so we look into adding constraints for
|
// We have a new completed submap, so we look into adding constraints for
|
||||||
// old scans.
|
// old scans.
|
||||||
|
@ -433,44 +454,32 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
||||||
if (trajectory_ids_.count(trajectory) == 0) {
|
if (trajectory_ids_.count(trajectory) == 0) {
|
||||||
return {transform::Rigid3d::Identity()};
|
return {transform::Rigid3d::Identity()};
|
||||||
}
|
}
|
||||||
const int trajectory_id = trajectory_ids_.at(trajectory);
|
const size_t trajectory_id = trajectory_ids_.at(trajectory);
|
||||||
size_t flat_index = 0;
|
if (trajectory_id >= submap_states_.size()) {
|
||||||
size_t flat_index_of_result_back = -1;
|
return {transform::Rigid3d::Identity()};
|
||||||
|
}
|
||||||
|
|
||||||
// Submaps for which we have optimized poses.
|
|
||||||
std::vector<transform::Rigid3d> result;
|
std::vector<transform::Rigid3d> result;
|
||||||
for (; flat_index != submap_states_.size(); ++flat_index) {
|
for (const auto& state : submap_states_.at(trajectory_id)) {
|
||||||
const auto& state = submap_states_[flat_index];
|
if (trajectory_id < submap_transforms.size() &&
|
||||||
if (state.id.trajectory_id != trajectory_id) {
|
result.size() < submap_transforms.at(trajectory_id).size()) {
|
||||||
continue;
|
// Submaps for which we have optimized poses.
|
||||||
}
|
|
||||||
if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
|
|
||||||
result.size() >= submap_transforms.at(trajectory_id).size()) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
result.push_back(
|
result.push_back(
|
||||||
submap_transforms.at(trajectory_id).at(result.size()).pose);
|
submap_transforms.at(trajectory_id).at(result.size()).pose);
|
||||||
flat_index_of_result_back = flat_index;
|
} else if (result.empty()) {
|
||||||
}
|
|
||||||
|
|
||||||
// Extrapolate to the remaining submaps.
|
|
||||||
for (; flat_index != submap_states_.size(); ++flat_index) {
|
|
||||||
const auto& state = submap_states_[flat_index];
|
|
||||||
if (state.id.trajectory_id != trajectory_id) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
if (result.empty()) {
|
|
||||||
result.push_back(transform::Rigid3d::Identity());
|
result.push_back(transform::Rigid3d::Identity());
|
||||||
} else {
|
} else {
|
||||||
// Accessing local_pose() in Submaps is okay, since the member is const.
|
// Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
|
||||||
|
// is okay, since the member is const.
|
||||||
result.push_back(result.back() *
|
result.push_back(result.back() *
|
||||||
submap_states_[flat_index_of_result_back]
|
submap_states_.at(trajectory_id)
|
||||||
|
.at(result.size() - 1)
|
||||||
.submap->local_pose()
|
.submap->local_pose()
|
||||||
.inverse() *
|
.inverse() *
|
||||||
state.submap->local_pose());
|
state.submap->local_pose());
|
||||||
}
|
}
|
||||||
flat_index_of_result_back = flat_index;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (result.empty()) {
|
if (result.empty()) {
|
||||||
result.push_back(transform::Rigid3d::Identity());
|
result.push_back(transform::Rigid3d::Identity());
|
||||||
}
|
}
|
||||||
|
|
|
@ -110,22 +110,16 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// against this submap. Likewise, all new scans are matched against submaps
|
// against this submap. Likewise, all new scans are matched against submaps
|
||||||
// which are finished.
|
// which are finished.
|
||||||
bool finished = false;
|
bool finished = false;
|
||||||
|
|
||||||
mapping::SubmapId id;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
// Handles a new work item.
|
// Handles a new work item.
|
||||||
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
|
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
|
||||||
|
|
||||||
int GetSubmapIndex(const mapping::Submap* submap) const REQUIRES(mutex_) {
|
|
||||||
const auto iterator = submap_indices_.find(submap);
|
|
||||||
CHECK(iterator != submap_indices_.end());
|
|
||||||
return iterator->second;
|
|
||||||
}
|
|
||||||
|
|
||||||
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
|
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
|
||||||
REQUIRES(mutex_) {
|
REQUIRES(mutex_) {
|
||||||
return submap_states_.at(GetSubmapIndex(submap)).id;
|
const auto iterator = submap_ids_.find(submap);
|
||||||
|
CHECK(iterator != submap_ids_.end());
|
||||||
|
return iterator->second;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Grows the optimization problem to have an entry for every element of
|
// Grows the optimization problem to have an entry for every element of
|
||||||
|
@ -141,8 +135,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
|
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
|
||||||
|
|
||||||
// Computes constraints for a scan and submap pair.
|
// Computes constraints for a scan and submap pair.
|
||||||
void ComputeConstraint(const int scan_index, const int submap_index)
|
void ComputeConstraint(const int scan_index,
|
||||||
REQUIRES(mutex_);
|
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
|
||||||
|
|
||||||
// Adds constraints for older scans whenever a new submap is finished.
|
// Adds constraints for older scans whenever a new submap is finished.
|
||||||
void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_);
|
void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_);
|
||||||
|
@ -191,11 +185,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
|
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
|
||||||
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
|
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Submaps get assigned an index and state as soon as they are seen, even
|
// Submaps get assigned an ID and state as soon as they are seen, even
|
||||||
// before they take part in the background computations.
|
// before they take part in the background computations.
|
||||||
std::map<const mapping::Submap*, int> submap_indices_ GUARDED_BY(mutex_);
|
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
||||||
std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
std::map<int, int> num_submaps_in_trajectory_ GUARDED_BY(mutex_);
|
std::vector<std::vector<SubmapState>> submap_states_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Mapping to flat indices to aid the transition to per-trajectory data
|
// Mapping to flat indices to aid the transition to per-trajectory data
|
||||||
// structures.
|
// structures.
|
||||||
|
|
Loading…
Reference in New Issue