12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Introduce PoseGraphModel and use it in 2D. (#1185) 

[RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0022-pose-graph-data.md)
master
Alexander Belyaev 2018-06-08 11:52:08 +02:00 committed by GitHub
parent 47a25e9e6f
commit 3437b931dd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 239 additions and 179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

273
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -61,27 +61,29 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one. // If we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
if (initial_trajectory_poses_.count(trajectory_id) > 0) { if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
trajectory_connectivity_state_.Connect( data_.trajectory_connectivity_state.Connect(
trajectory_id, trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time); data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
time);
} }
optimization_problem_->AddSubmap( optimization_problem_->AddSubmap(
trajectory_id, trajectory_id, transform::Project2D(
transform::Project2D(ComputeLocalToGlobalTransform( ComputeLocalToGlobalTransform(
global_submap_poses_, trajectory_id) * data_.global_submap_poses_2d, trajectory_id) *
insertion_submaps[0]->local_pose())); insertion_submaps[0]->local_pose()));
} }
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const SubmapId submap_id{trajectory_id, 0}; const SubmapId submap_id{trajectory_id, 0};
CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
return {submap_id}; return {submap_id};
} }
CHECK_EQ(2, insertion_submaps.size()); CHECK_EQ(2, insertion_submaps.size());
const auto end_it = submap_data.EndOfTrajectory(trajectory_id); const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
const SubmapId last_submap_id = std::prev(end_it)->id; const SubmapId last_submap_id = std::prev(end_it)->id;
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { if (data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of // In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new. // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose; const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
@ -93,10 +95,12 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
return {last_submap_id, return {last_submap_id,
SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
} }
CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back()); CHECK(data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.back());
const SubmapId front_submap_id{trajectory_id, const SubmapId front_submap_id{trajectory_id,
last_submap_id.submap_index - 1}; last_submap_id.submap_index - 1};
CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front()); CHECK(data_.submap_data.at(front_submap_id).submap ==
insertion_submaps.front());
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
@ -109,19 +113,19 @@ NodeId PoseGraph2D::AddNode(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id); AddTrajectoryIfNeeded(trajectory_id);
const NodeId node_id = trajectory_nodes_.Append( const NodeId node_id = data_.trajectory_nodes.Append(
trajectory_id, TrajectoryNode{constant_data, optimized_pose}); trajectory_id, TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_; ++data_.num_trajectory_nodes;
// Test if the 'insertion_submap.back()' is one we never saw before. // Test if the 'insertion_submap.back()' is one we never saw before.
if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 || if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap != std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
insertion_submaps.back()) { ->data.submap != insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first // We grow 'data_.submap_data' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'. // time we see a new submap is as 'insertion_submaps.back()'.
const SubmapId submap_id = const SubmapId submap_id =
submap_data_.Append(trajectory_id, InternalSubmapData()); data_.submap_data.Append(trajectory_id, InternalSubmapData());
submap_data_.at(submap_id).submap = insertion_submaps.back(); data_.submap_data.at(submap_id).submap = insertion_submaps.back();
} }
// We have to check this here, because it might have changed by the time we // We have to check this here, because it might have changed by the time we
@ -143,7 +147,7 @@ void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
} }
void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
trajectory_connectivity_state_.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
// Make sure we have a sampler for this trajectory. // Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) { if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory_id] = global_localization_samplers_[trajectory_id] =
@ -180,7 +184,7 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id,
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
for (const auto& observation : landmark_data.landmark_observations) { for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back( data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
LandmarkNode::LandmarkObservation{ LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time, trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform, observation.landmark_to_tracking_transform,
@ -191,11 +195,11 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id,
void PoseGraph2D::ComputeConstraint(const NodeId& node_id, void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const SubmapId& submap_id) { const SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
const common::Time node_time = GetLatestNodeTime(node_id, submap_id); const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
const common::Time last_connection_time = const common::Time last_connection_time =
trajectory_connectivity_state_.LastConnectionTime( data_.trajectory_connectivity_state.LastConnectionTime(
node_id.trajectory_id, submap_id.trajectory_id); node_id.trajectory_id, submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id || if (node_id.trajectory_id == submap_id.trajectory_id ||
node_time < node_time <
@ -212,18 +216,22 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
.global_pose.inverse() * .global_pose.inverse() *
optimization_problem_->node_data().at(node_id).global_pose_2d; optimization_problem_->node_data().at(node_id).global_pose_2d;
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, submap_id,
trajectory_nodes_.at(node_id).constant_data.get(), static_cast<const Submap2D*>(
data_.submap_data.at(submap_id).submap.get()),
node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
initial_relative_pose); initial_relative_pose);
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, submap_id,
trajectory_nodes_.at(node_id).constant_data.get()); static_cast<const Submap2D*>(
data_.submap_data.at(submap_id).submap.get()),
node_id, data_.trajectory_nodes.at(node_id).constant_data.get());
} }
} }
void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = data_.submap_data.at(submap_id);
for (const auto& node_id_data : optimization_problem_->node_data()) { for (const auto& node_id_data : optimization_problem_->node_data()) {
const NodeId& node_id = node_id_data.id; const NodeId& node_id = node_id_data.id;
if (submap_data.node_ids.count(node_id) == 0) { if (submap_data.node_ids.count(node_id) == 0) {
@ -236,7 +244,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps, std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data;
const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses( const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps); node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size()); CHECK_EQ(submap_ids.size(), insertion_submaps.size());
@ -256,13 +264,14 @@ void PoseGraph2D::ComputeConstraintsForNode(
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i]; const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will // Even if this was the last node added to 'submap_id', the submap will
// only be marked as finished in 'submap_data_' further below. // only be marked as finished in 'data_.submap_data' further below.
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id); data_.submap_data.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform = const transform::Rigid2d constraint_transform =
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d; local_pose_2d;
constraints_.push_back(Constraint{submap_id, data_.constraints.push_back(
Constraint{submap_id,
node_id, node_id,
{transform::Embed3D(constraint_transform), {transform::Embed3D(constraint_transform),
options_.matcher_translation_weight(), options_.matcher_translation_weight(),
@ -270,7 +279,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
Constraint::INTRA_SUBMAP}); Constraint::INTRA_SUBMAP});
} }
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : data_.submap_data) {
if (submap_id_data.data.state == SubmapState::kFinished) { if (submap_id_data.data.state == SubmapState::kFinished) {
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
ComputeConstraint(node_id, submap_id_data.id); ComputeConstraint(node_id, submap_id_data.id);
@ -280,7 +289,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
if (newly_finished_submap) { if (newly_finished_submap) {
const SubmapId finished_submap_id = submap_ids.front(); const SubmapId finished_submap_id = submap_ids.front();
InternalSubmapData& finished_submap_data = InternalSubmapData& finished_submap_data =
submap_data_.at(finished_submap_id); data_.submap_data.at(finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);
finished_submap_data.state = SubmapState::kFinished; finished_submap_data.state = SubmapState::kFinished;
// 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
@ -307,13 +316,14 @@ void PoseGraph2D::DispatchOptimization() {
} }
common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id, common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const { const SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time; common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time;
const InternalSubmapData& submap_data = submap_data_.at(submap_id); const InternalSubmapData& submap_data = data_.submap_data.at(submap_id);
if (!submap_data.node_ids.empty()) { if (!submap_data.node_ids.empty()) {
const NodeId last_submap_node_id = const NodeId last_submap_node_id =
*submap_data_.at(submap_id).node_ids.rbegin(); *data_.submap_data.at(submap_id).node_ids.rbegin();
time = std::max( time = std::max(
time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); time,
data_.trajectory_nodes.at(last_submap_node_id).constant_data->time);
} }
return time; return time;
} }
@ -322,8 +332,8 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
const common::Time time = const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id); GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, data_.trajectory_connectivity_state.Connect(
constraint.submap_id.trajectory_id, constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id,
time); time);
} }
@ -331,7 +341,8 @@ void PoseGraph2D::HandleWorkQueue(
const constraints::ConstraintBuilder2D::Result& result) { const constraints::ConstraintBuilder2D::Result& result) {
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
} }
RunOptimization(); RunOptimization();
@ -393,18 +404,19 @@ void PoseGraph2D::WaitForAllComputations() {
while (!locker.AwaitWithTimeout( while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { [this]() REQUIRES(mutex_) {
return ((constraint_builder_.GetNumFinishedNodes() == return ((constraint_builder_.GetNumFinishedNodes() ==
num_trajectory_nodes_) && data_.num_trajectory_nodes) &&
!work_queue_); !work_queue_);
}, },
common::FromSeconds(1.))) { common::FromSeconds(1.))) {
// Log progress on nodes only when we are actually processing nodes. // Log progress on nodes only when we are actually processing nodes.
if (num_trajectory_nodes_ != num_finished_nodes_at_start) { if (data_.num_trajectory_nodes != num_finished_nodes_at_start) {
std::ostringstream progress_info; std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1) progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * << 100. *
(constraint_builder_.GetNumFinishedNodes() - (constraint_builder_.GetNumFinishedNodes() -
num_finished_nodes_at_start) / num_finished_nodes_at_start) /
(num_trajectory_nodes_ - num_finished_nodes_at_start) (data_.num_trajectory_nodes -
num_finished_nodes_at_start)
<< "%..."; << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush; std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
} }
@ -414,7 +426,8 @@ void PoseGraph2D::WaitForAllComputations() {
[this, [this,
&notification](const constraints::ConstraintBuilder2D::Result& result) { &notification](const constraints::ConstraintBuilder2D::Result& result) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
notification = true; notification = true;
}); });
locker.Await([&notification]() { return notification; }); locker.Await([&notification]() { return notification; });
@ -423,11 +436,11 @@ void PoseGraph2D::WaitForAllComputations() {
void PoseGraph2D::FinishTrajectory(const int trajectory_id) { void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(finished_trajectories_.count(trajectory_id), 0); CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0);
finished_trajectories_.insert(trajectory_id); data_.finished_trajectories.insert(trajectory_id);
for (const auto& submap : submap_data_.trajectory(trajectory_id)) { for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
submap_data_.at(submap.id).state = SubmapState::kFinished; data_.submap_data.at(submap.id).state = SubmapState::kFinished;
} }
CHECK(!run_loop_closure_); CHECK(!run_loop_closure_);
DispatchOptimization(); DispatchOptimization();
@ -435,20 +448,20 @@ void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
} }
bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const { bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const {
return finished_trajectories_.count(trajectory_id) > 0; return data_.finished_trajectories.count(trajectory_id) > 0;
} }
void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id); data_.trajectory_connectivity_state.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0); CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0);
frozen_trajectories_.insert(trajectory_id); data_.frozen_trajectories.insert(trajectory_id);
}); });
} }
bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const { bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const {
return frozen_trajectories_.count(trajectory_id) > 0; return data_.frozen_trajectories.count(trajectory_id) > 0;
} }
void PoseGraph2D::AddSubmapFromProto( void PoseGraph2D::AddSubmapFromProto(
@ -466,13 +479,13 @@ void PoseGraph2D::AddSubmapFromProto(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(submap_id.trajectory_id); AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, InternalSubmapData()); data_.submap_data.Insert(submap_id, InternalSubmapData());
submap_data_.at(submap_id).submap = submap_ptr; data_.submap_data.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'. // Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert( data_.global_submap_poses_2d.Insert(
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished; data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
}); });
} }
@ -486,10 +499,12 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(node_id.trajectory_id); AddTrajectoryIfNeeded(node_id.trajectory_id);
trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose}); data_.trajectory_nodes.Insert(node_id,
TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()); constant_data->gravity_alignment.inverse());
optimization_problem_->InsertTrajectoryNode( optimization_problem_->InsertTrajectoryNode(
@ -512,7 +527,7 @@ void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) { const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
submap_data_.at(submap_id).node_ids.insert(node_id); data_.submap_data.at(submap_id).node_ids.insert(node_id);
}); });
} }
@ -521,13 +536,14 @@ void PoseGraph2D::AddSerializedConstraints(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() REQUIRES(mutex_) { AddWorkItem([this, constraints]() REQUIRES(mutex_) {
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
CHECK(trajectory_nodes_.Contains(constraint.node_id)); CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
CHECK(submap_data_.Contains(constraint.submap_id)); CHECK(data_.submap_data.Contains(constraint.submap_id));
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr); CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data !=
CHECK(submap_data_.at(constraint.submap_id).submap != nullptr); nullptr);
CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr);
switch (constraint.tag) { switch (constraint.tag) {
case Constraint::Tag::INTRA_SUBMAP: case Constraint::Tag::INTRA_SUBMAP:
CHECK(submap_data_.at(constraint.submap_id) CHECK(data_.submap_data.at(constraint.submap_id)
.node_ids.emplace(constraint.node_id) .node_ids.emplace(constraint.node_id)
.second); .second);
break; break;
@ -538,10 +554,10 @@ void PoseGraph2D::AddSerializedConstraints(
const Constraint::Pose pose = { const Constraint::Pose pose = {
constraint.pose.zbar_ij * constraint.pose.zbar_ij *
transform::Rigid3d::Rotation( transform::Rigid3d::Rotation(
trajectory_nodes_.at(constraint.node_id) data_.trajectory_nodes.at(constraint.node_id)
.constant_data->gravity_alignment.inverse()), .constant_data->gravity_alignment.inverse()),
constraint.pose.translation_weight, constraint.pose.rotation_weight}; constraint.pose.translation_weight, constraint.pose.rotation_weight};
constraints_.push_back(Constraint{ data_.constraints.push_back(Constraint{
constraint.submap_id, constraint.node_id, pose, constraint.tag}); constraint.submap_id, constraint.node_id, pose, constraint.tag});
} }
LOG(INFO) << "Loaded " << constraints.size() << " constraints."; LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
@ -579,19 +595,19 @@ void PoseGraph2D::RunOptimization() {
return; return;
} }
// No other thread is accessing the optimization_problem_, constraints_, // No other thread is accessing the optimization_problem_,
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
// time consuming, so not taking the mutex before Solve to avoid blocking // when executing the Solve. Solve is time consuming, so not taking the mutex
// foreground processing. // before Solve to avoid blocking foreground processing.
optimization_problem_->Solve(constraints_, frozen_trajectories_, optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories,
landmark_nodes_); data_.landmark_nodes);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data(); const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data(); const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids()) { for (const int trajectory_id : node_data.trajectory_ids()) {
for (const auto& node : node_data.trajectory(trajectory_id)) { for (const auto& node : node_data.trajectory(trajectory_id)) {
auto& mutable_trajectory_node = trajectory_nodes_.at(node.id); auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
mutable_trajectory_node.global_pose = mutable_trajectory_node.global_pose =
transform::Embed3D(node.data.global_pose_2d) * transform::Embed3D(node.data.global_pose_2d) *
transform::Rigid3d::Rotation( transform::Rigid3d::Rotation(
@ -602,37 +618,38 @@ void PoseGraph2D::RunOptimization() {
// 'optimization_problem_' yet. // 'optimization_problem_' yet.
const auto local_to_new_global = const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id); ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = const auto local_to_old_global = ComputeLocalToGlobalTransform(
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); data_.global_submap_poses_2d, trajectory_id);
const transform::Rigid3d old_global_to_new_global = const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse(); local_to_new_global * local_to_old_global.inverse();
const NodeId last_optimized_node_id = const NodeId last_optimized_node_id =
std::prev(node_data.EndOfTrajectory(trajectory_id))->id; std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id)); auto node_it =
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); std::next(data_.trajectory_nodes.find(last_optimized_node_id));
for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
++node_it) { ++node_it) {
auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id); auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
mutable_trajectory_node.global_pose = mutable_trajectory_node.global_pose =
old_global_to_new_global * mutable_trajectory_node.global_pose; old_global_to_new_global * mutable_trajectory_node.global_pose;
} }
} }
for (const auto& landmark : optimization_problem_->landmark_data()) { for (const auto& landmark : optimization_problem_->landmark_data()) {
landmark_nodes_[landmark.first].global_landmark_pose = landmark.second; data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
} }
global_submap_poses_ = submap_data; data_.global_submap_poses_2d = submap_data;
} }
MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const { MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return trajectory_nodes_; return data_.trajectory_nodes;
} }
MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses() MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
const { const {
MapById<NodeId, TrajectoryNodePose> node_poses; MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const auto& node_id_data : trajectory_nodes_) { for (const auto& node_id_data : data_.trajectory_nodes) {
common::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data; common::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
if (node_id_data.data.constant_data != nullptr) { if (node_id_data.data.constant_data != nullptr) {
constant_pose_data = TrajectoryNodePose::ConstantPoseData{ constant_pose_data = TrajectoryNodePose::ConstantPoseData{
@ -650,7 +667,7 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
const { const {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const auto& landmark : landmark_nodes_) { for (const auto& landmark : data_.landmark_nodes) {
// Landmark without value has not been optimized yet. // Landmark without value has not been optimized yet.
if (!landmark.second.global_landmark_pose.has_value()) continue; if (!landmark.second.global_landmark_pose.has_value()) continue;
landmark_poses[landmark.first] = landmark_poses[landmark.first] =
@ -663,7 +680,7 @@ void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) { const transform::Rigid3d& global_pose) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
landmark_nodes_[landmark_id].global_landmark_pose = global_pose; data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
}); });
} }
@ -680,7 +697,7 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() const {
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph2D::GetLandmarkNodes() const { PoseGraph2D::GetLandmarkNodes() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return landmark_nodes_; return data_.landmark_nodes;
} }
std::map<int, PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
@ -696,12 +713,12 @@ PoseGraph2D::GetFixedFramePoseData() const {
std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const { std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const {
std::vector<PoseGraphInterface::Constraint> result; std::vector<PoseGraphInterface::Constraint> result;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : constraints_) { for (const Constraint& constraint : data_.constraints) {
result.push_back(Constraint{ result.push_back(Constraint{
constraint.submap_id, constraint.node_id, constraint.submap_id, constraint.node_id,
Constraint::Pose{constraint.pose.zbar_ij * Constraint::Pose{constraint.pose.zbar_ij *
transform::Rigid3d::Rotation( transform::Rigid3d::Rotation(
trajectory_nodes_.at(constraint.node_id) data_.trajectory_nodes.at(constraint.node_id)
.constant_data->gravity_alignment), .constant_data->gravity_alignment),
constraint.pose.translation_weight, constraint.pose.translation_weight,
constraint.pose.rotation_weight}, constraint.pose.rotation_weight},
@ -715,19 +732,20 @@ void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) { const common::Time time) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
initial_trajectory_poses_[from_trajectory_id] = data_.initial_trajectory_poses[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time}; InitialTrajectoryPose{to_trajectory_id, pose, time};
} }
transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
const int trajectory_id, const common::Time time) const { const int trajectory_id, const common::Time time) const {
CHECK_GT(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id), 0); CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0);
const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time);
if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) { if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) {
return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose; return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)
->data.global_pose;
} }
if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) { if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) {
return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id)) return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
->data.global_pose; ->data.global_pose;
} }
return transform::Interpolate( return transform::Interpolate(
@ -742,11 +760,12 @@ transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
const int trajectory_id) const { const int trajectory_id) const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
trajectory_id);
} }
std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const { std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const {
return trajectory_connectivity_state_.Components(); return data_.trajectory_connectivity_state.Components();
} }
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData( PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(
@ -765,7 +784,7 @@ MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph2D::GetAllSubmapPoses() const { PoseGraph2D::GetAllSubmapPoses() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
MapById<SubmapId, SubmapPose> submap_poses; MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : data_.submap_data) {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
submap_poses.Insert( submap_poses.Insert(
submap_id_data.id, submap_id_data.id,
@ -781,8 +800,8 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) { if (begin_it == end_it) {
const auto it = initial_trajectory_poses_.find(trajectory_id); const auto it = data_.initial_trajectory_poses.find(trajectory_id);
if (it != initial_trajectory_poses_.end()) { if (it != data_.initial_trajectory_poses.end()) {
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
it->second.time) * it->second.time) *
it->second.relative_pose; it->second.relative_pose;
@ -794,25 +813,26 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
// Accessing 'local_pose' in Submap is okay, since the member is const. // Accessing 'local_pose' in Submap is okay, since the member is const.
return transform::Embed3D( return transform::Embed3D(
global_submap_poses.at(last_optimized_submap_id).global_pose) * global_submap_poses.at(last_optimized_submap_id).global_pose) *
submap_data_.at(last_optimized_submap_id) data_.submap_data.at(last_optimized_submap_id)
.submap->local_pose() .submap->local_pose()
.inverse(); .inverse();
} }
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock( PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const SubmapId& submap_id) const { const SubmapId& submap_id) const {
const auto it = submap_data_.find(submap_id); const auto it = data_.submap_data.find(submap_id);
if (it == submap_data_.end()) { if (it == data_.submap_data.end()) {
return {}; return {};
} }
auto submap = it->data.submap; auto submap = it->data.submap;
if (global_submap_poses_.Contains(submap_id)) { if (data_.global_submap_poses_2d.Contains(submap_id)) {
// We already have an optimized pose. // We already have an optimized pose.
return {submap, return {submap,
transform::Embed3D(global_submap_poses_.at(submap_id).global_pose)}; transform::Embed3D(
data_.global_submap_poses_2d.at(submap_id).global_pose)};
} }
// We have to extrapolate. // We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(global_submap_poses_, return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
submap_id.trajectory_id) * submap_id.trajectory_id) *
submap->local_pose()}; submap->local_pose()};
} }
@ -828,14 +848,15 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const { PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const {
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps; MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : parent_->submap_data_) { for (const auto& submap_id_data : parent_->data_.submap_data) {
if (submap_id_data.data.state != SubmapState::kFinished || if (submap_id_data.data.state != SubmapState::kFinished ||
!parent_->global_submap_poses_.Contains(submap_id_data.id)) { !parent_->data_.global_submap_poses_2d.Contains(submap_id_data.id)) {
continue; continue;
} }
submaps.Insert(submap_id_data.id, submaps.Insert(
submap_id_data.id,
SubmapData{submap_id_data.data.submap, SubmapData{submap_id_data.data.submap,
transform::Embed3D(parent_->global_submap_poses_ transform::Embed3D(parent_->data_.global_submap_poses_2d
.at(submap_id_data.id) .at(submap_id_data.id)
.global_pose)}); .global_pose)});
} }
@ -854,12 +875,12 @@ std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
const MapById<NodeId, TrajectoryNode>& const MapById<NodeId, TrajectoryNode>&
PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const { PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {
return parent_->trajectory_nodes_; return parent_->data_.trajectory_nodes;
} }
const std::vector<PoseGraphInterface::Constraint>& const std::vector<PoseGraphInterface::Constraint>&
PoseGraph2D::TrimmingHandle::GetConstraints() const { PoseGraph2D::TrimmingHandle::GetConstraints() const {
return parent_->constraints_; return parent_->data_.constraints;
} }
bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const { bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {
@ -870,22 +891,23 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
const SubmapId& submap_id) { const SubmapId& submap_id) {
// TODO(hrapp): We have to make sure that the trajectory has been finished // TODO(hrapp): We have to make sure that the trajectory has been finished
// if we want to delete the last submaps. // if we want to delete the last submaps.
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(parent_->data_.submap_data.at(submap_id).state ==
SubmapState::kFinished);
// Compile all nodes that are still INTRA_SUBMAP constrained once the submap // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
// with 'submap_id' is gone. // with 'submap_id' is gone.
std::set<NodeId> nodes_to_retain; std::set<NodeId> nodes_to_retain;
for (const Constraint& constraint : parent_->constraints_) { for (const Constraint& constraint : parent_->data_.constraints) {
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
constraint.submap_id != submap_id) { constraint.submap_id != submap_id) {
nodes_to_retain.insert(constraint.node_id); nodes_to_retain.insert(constraint.node_id);
} }
} }
// Remove all 'constraints_' related to 'submap_id'. // Remove all 'data_.constraints' related to 'submap_id'.
std::set<NodeId> nodes_to_remove; std::set<NodeId> nodes_to_remove;
{ {
std::vector<Constraint> constraints; std::vector<Constraint> constraints;
for (const Constraint& constraint : parent_->constraints_) { for (const Constraint& constraint : parent_->data_.constraints) {
if (constraint.submap_id == submap_id) { if (constraint.submap_id == submap_id) {
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
nodes_to_retain.count(constraint.node_id) == 0) { nodes_to_retain.count(constraint.node_id) == 0) {
@ -897,29 +919,30 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
constraints.push_back(constraint); constraints.push_back(constraint);
} }
} }
parent_->constraints_ = std::move(constraints); parent_->data_.constraints = std::move(constraints);
} }
// Remove all 'constraints_' related to 'nodes_to_remove'. // Remove all 'data_.constraints' related to 'nodes_to_remove'.
{ {
std::vector<Constraint> constraints; std::vector<Constraint> constraints;
for (const Constraint& constraint : parent_->constraints_) { for (const Constraint& constraint : parent_->data_.constraints) {
if (nodes_to_remove.count(constraint.node_id) == 0) { if (nodes_to_remove.count(constraint.node_id) == 0) {
constraints.push_back(constraint); constraints.push_back(constraint);
} }
} }
parent_->constraints_ = std::move(constraints); parent_->data_.constraints = std::move(constraints);
} }
// Mark the submap with 'submap_id' as trimmed and remove its data. // Mark the submap with 'submap_id' as trimmed and remove its data.
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(parent_->data_.submap_data.at(submap_id).state ==
parent_->submap_data_.Trim(submap_id); SubmapState::kFinished);
parent_->data_.submap_data.Trim(submap_id);
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_->TrimSubmap(submap_id); parent_->optimization_problem_->TrimSubmap(submap_id);
// Remove the 'nodes_to_remove' from the pose graph and the optimization // Remove the 'nodes_to_remove' from the pose graph and the optimization
// problem. // problem.
for (const NodeId& node_id : nodes_to_remove) { for (const NodeId& node_id : nodes_to_remove) {
parent_->trajectory_nodes_.Trim(node_id); parent_->data_.trajectory_nodes.Trim(node_id);
parent_->optimization_problem_->TrimTrajectoryNode(node_id); parent_->optimization_problem_->TrimTrajectoryNode(node_id);
} }
} }
@ -927,7 +950,7 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetSubmapDataUnderLock() const { PoseGraph2D::GetSubmapDataUnderLock() const {
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps; MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : data_.submap_data) {
submaps.Insert(submap_id_data.id, submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id)); GetSubmapDataUnderLock(submap_id_data.id));
} }

46
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -37,6 +37,7 @@
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_data.h"
#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
@ -146,21 +147,6 @@ class PoseGraph2D : public PoseGraph {
int trajectory_id, const common::Time time) const REQUIRES(mutex_); int trajectory_id, const common::Time time) const REQUIRES(mutex_);
private: private:
// The current state of the submap in the background threads. When this
// transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished };
struct InternalSubmapData {
std::shared_ptr<const Submap2D> submap;
// IDs of the nodes that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
// becomes 'finished'.
std::set<NodeId> node_ids;
SubmapState state = SubmapState::kActive;
};
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock() MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
const REQUIRES(mutex_); const REQUIRES(mutex_);
@ -229,9 +215,6 @@ class PoseGraph2D : public PoseGraph {
std::unique_ptr<std::deque<std::function<void()>>> work_queue_ std::unique_ptr<std::deque<std::function<void()>>> work_queue_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// How our various trajectories are related.
TrajectoryConnectivityState trajectory_connectivity_state_;
// We globally localize a fraction of the nodes from each trajectory. // We globally localize a fraction of the nodes from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>> std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
global_localization_samplers_ GUARDED_BY(mutex_); global_localization_samplers_ GUARDED_BY(mutex_);
@ -248,36 +231,11 @@ class PoseGraph2D : public PoseGraph {
// Current optimization problem. // Current optimization problem.
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
constraints::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); constraints::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.
MapById<SubmapId, InternalSubmapData> submap_data_ GUARDED_BY(mutex_);
// Data that are currently being shown.
MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data.
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_
GUARDED_BY(mutex_);
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
landmark_nodes_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish. // List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_); std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);
// Set of all frozen trajectories not being optimized. PoseGraphData data_ GUARDED_BY(mutex_);
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
// Set of all finished trajectories.
std::set<int> finished_trajectories_ GUARDED_BY(mutex_);
// Set of all initial trajectory poses.
std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
GUARDED_BY(mutex_);
// Allows querying and manipulating the pose graph by the 'trimmers_'. The // Allows querying and manipulating the pose graph by the 'trimmers_'. The
// 'mutex_' of the pose graph is held while this class is used. // 'mutex_' of the pose graph is held while this class is used.

79
cartographer/mapping/pose_graph_data.h Normal file
View File

@ -0,0 +1,79 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_
#include <map>
#include <set>
#include <vector>
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/submaps.h"
namespace cartographer {
namespace mapping {
// The current state of the submap in the background threads. When this
// transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished };
struct InternalSubmapData {
std::shared_ptr<const Submap> submap;
SubmapState state = SubmapState::kActive;
// IDs of the nodes that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
// becomes 'finished'.
std::set<NodeId> node_ids;
};
struct PoseGraphData {
// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.
MapById<SubmapId, InternalSubmapData> submap_data;
// Global submap poses currently used for displaying data.
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;
// Data that are currently being shown.
MapById<NodeId, TrajectoryNode> trajectory_nodes;
// Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
landmark_nodes;
// How our various trajectories are related.
TrajectoryConnectivityState trajectory_connectivity_state;
int num_trajectory_nodes = 0;
std::set<int> finished_trajectories;
std::set<int> frozen_trajectories;
// Set of all initial trajectory poses.
std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;
std::vector<PoseGraphInterface::Constraint> constraints;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_