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
GitHub
parent
47a25e9e6f
commit
3437b931dd
|
|
@ -61,27 +61,29 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
|
|||
if (insertion_submaps.size() == 1) {
|
||||
// If we don't already have an entry for the first submap, add one.
|
||||
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
|
||||
if (initial_trajectory_poses_.count(trajectory_id) > 0) {
|
||||
trajectory_connectivity_state_.Connect(
|
||||
if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
|
||||
data_.trajectory_connectivity_state.Connect(
|
||||
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(
|
||||
trajectory_id,
|
||||
transform::Project2D(ComputeLocalToGlobalTransform(
|
||||
global_submap_poses_, trajectory_id) *
|
||||
insertion_submaps[0]->local_pose()));
|
||||
trajectory_id, transform::Project2D(
|
||||
ComputeLocalToGlobalTransform(
|
||||
data_.global_submap_poses_2d, trajectory_id) *
|
||||
insertion_submaps[0]->local_pose()));
|
||||
}
|
||||
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
|
||||
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};
|
||||
}
|
||||
CHECK_EQ(2, insertion_submaps.size());
|
||||
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
|
||||
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
|
||||
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
|
||||
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
|
||||
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,
|
||||
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,
|
||||
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};
|
||||
}
|
||||
|
||||
|
|
@ -109,19 +113,19 @@ NodeId PoseGraph2D::AddNode(
|
|||
|
||||
common::MutexLocker locker(&mutex_);
|
||||
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});
|
||||
++num_trajectory_nodes_;
|
||||
++data_.num_trajectory_nodes;
|
||||
|
||||
// Test if the 'insertion_submap.back()' is one we never saw before.
|
||||
if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
|
||||
std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
|
||||
insertion_submaps.back()) {
|
||||
// We grow 'submap_data_' as needed. This code assumes that the first
|
||||
if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
|
||||
std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
|
||||
->data.submap != insertion_submaps.back()) {
|
||||
// We grow 'data_.submap_data' as needed. This code assumes that the first
|
||||
// time we see a new submap is as 'insertion_submaps.back()'.
|
||||
const SubmapId submap_id =
|
||||
submap_data_.Append(trajectory_id, InternalSubmapData());
|
||||
submap_data_.at(submap_id).submap = insertion_submaps.back();
|
||||
data_.submap_data.Append(trajectory_id, InternalSubmapData());
|
||||
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
|
||||
|
|
@ -143,7 +147,7 @@ void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
|
|||
}
|
||||
|
||||
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.
|
||||
if (!global_localization_samplers_[trajectory_id]) {
|
||||
global_localization_samplers_[trajectory_id] =
|
||||
|
|
@ -180,7 +184,7 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id,
|
|||
common::MutexLocker locker(&mutex_);
|
||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||
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{
|
||||
trajectory_id, landmark_data.time,
|
||||
observation.landmark_to_tracking_transform,
|
||||
|
|
@ -191,11 +195,11 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id,
|
|||
|
||||
void PoseGraph2D::ComputeConstraint(const NodeId& node_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 last_connection_time =
|
||||
trajectory_connectivity_state_.LastConnectionTime(
|
||||
data_.trajectory_connectivity_state.LastConnectionTime(
|
||||
node_id.trajectory_id, submap_id.trajectory_id);
|
||||
if (node_id.trajectory_id == submap_id.trajectory_id ||
|
||||
node_time <
|
||||
|
|
@ -212,18 +216,22 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
|
|||
.global_pose.inverse() *
|
||||
optimization_problem_->node_data().at(node_id).global_pose_2d;
|
||||
constraint_builder_.MaybeAddConstraint(
|
||||
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
|
||||
trajectory_nodes_.at(node_id).constant_data.get(),
|
||||
submap_id,
|
||||
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);
|
||||
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
|
||||
constraint_builder_.MaybeAddGlobalConstraint(
|
||||
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
|
||||
trajectory_nodes_.at(node_id).constant_data.get());
|
||||
submap_id,
|
||||
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) {
|
||||
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()) {
|
||||
const NodeId& node_id = node_id_data.id;
|
||||
if (submap_data.node_ids.count(node_id) == 0) {
|
||||
|
|
@ -236,7 +244,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
|
|||
const NodeId& node_id,
|
||||
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
|
||||
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(
|
||||
node_id.trajectory_id, constant_data->time, insertion_submaps);
|
||||
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
|
||||
|
|
@ -256,21 +264,22 @@ void PoseGraph2D::ComputeConstraintsForNode(
|
|||
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
|
||||
const SubmapId submap_id = submap_ids[i];
|
||||
// Even if this was the last node added to 'submap_id', the submap will
|
||||
// only be marked as finished in 'submap_data_' further below.
|
||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
|
||||
submap_data_.at(submap_id).node_ids.emplace(node_id);
|
||||
// only be marked as finished in 'data_.submap_data' further below.
|
||||
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive);
|
||||
data_.submap_data.at(submap_id).node_ids.emplace(node_id);
|
||||
const transform::Rigid2d constraint_transform =
|
||||
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
|
||||
local_pose_2d;
|
||||
constraints_.push_back(Constraint{submap_id,
|
||||
node_id,
|
||||
{transform::Embed3D(constraint_transform),
|
||||
options_.matcher_translation_weight(),
|
||||
options_.matcher_rotation_weight()},
|
||||
Constraint::INTRA_SUBMAP});
|
||||
data_.constraints.push_back(
|
||||
Constraint{submap_id,
|
||||
node_id,
|
||||
{transform::Embed3D(constraint_transform),
|
||||
options_.matcher_translation_weight(),
|
||||
options_.matcher_rotation_weight()},
|
||||
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) {
|
||||
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
|
||||
ComputeConstraint(node_id, submap_id_data.id);
|
||||
|
|
@ -280,7 +289,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
|
|||
if (newly_finished_submap) {
|
||||
const SubmapId finished_submap_id = submap_ids.front();
|
||||
InternalSubmapData& finished_submap_data =
|
||||
submap_data_.at(finished_submap_id);
|
||||
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
|
||||
|
|
@ -307,13 +316,14 @@ void PoseGraph2D::DispatchOptimization() {
|
|||
}
|
||||
common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
|
||||
const SubmapId& submap_id) const {
|
||||
common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
|
||||
const InternalSubmapData& submap_data = submap_data_.at(submap_id);
|
||||
common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time;
|
||||
const InternalSubmapData& submap_data = data_.submap_data.at(submap_id);
|
||||
if (!submap_data.node_ids.empty()) {
|
||||
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, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
|
||||
time,
|
||||
data_.trajectory_nodes.at(last_submap_node_id).constant_data->time);
|
||||
}
|
||||
return time;
|
||||
}
|
||||
|
|
@ -322,16 +332,17 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
|
|||
CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
|
||||
const common::Time time =
|
||||
GetLatestNodeTime(constraint.node_id, constraint.submap_id);
|
||||
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
|
||||
constraint.submap_id.trajectory_id,
|
||||
time);
|
||||
data_.trajectory_connectivity_state.Connect(
|
||||
constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id,
|
||||
time);
|
||||
}
|
||||
|
||||
void PoseGraph2D::HandleWorkQueue(
|
||||
const constraints::ConstraintBuilder2D::Result& result) {
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||
data_.constraints.insert(data_.constraints.end(), result.begin(),
|
||||
result.end());
|
||||
}
|
||||
RunOptimization();
|
||||
|
||||
|
|
@ -393,18 +404,19 @@ void PoseGraph2D::WaitForAllComputations() {
|
|||
while (!locker.AwaitWithTimeout(
|
||||
[this]() REQUIRES(mutex_) {
|
||||
return ((constraint_builder_.GetNumFinishedNodes() ==
|
||||
num_trajectory_nodes_) &&
|
||||
data_.num_trajectory_nodes) &&
|
||||
!work_queue_);
|
||||
},
|
||||
common::FromSeconds(1.))) {
|
||||
// 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;
|
||||
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
|
||||
<< 100. *
|
||||
(constraint_builder_.GetNumFinishedNodes() -
|
||||
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;
|
||||
}
|
||||
|
|
@ -414,7 +426,8 @@ void PoseGraph2D::WaitForAllComputations() {
|
|||
[this,
|
||||
¬ification](const constraints::ConstraintBuilder2D::Result& result) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||
data_.constraints.insert(data_.constraints.end(), result.begin(),
|
||||
result.end());
|
||||
notification = true;
|
||||
});
|
||||
locker.Await([¬ification]() { return notification; });
|
||||
|
|
@ -423,11 +436,11 @@ void PoseGraph2D::WaitForAllComputations() {
|
|||
void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
|
||||
CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
|
||||
finished_trajectories_.insert(trajectory_id);
|
||||
CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0);
|
||||
data_.finished_trajectories.insert(trajectory_id);
|
||||
|
||||
for (const auto& submap : submap_data_.trajectory(trajectory_id)) {
|
||||
submap_data_.at(submap.id).state = SubmapState::kFinished;
|
||||
for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
|
||||
data_.submap_data.at(submap.id).state = SubmapState::kFinished;
|
||||
}
|
||||
CHECK(!run_loop_closure_);
|
||||
DispatchOptimization();
|
||||
|
|
@ -435,20 +448,20 @@ void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
|
|||
}
|
||||
|
||||
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) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
trajectory_connectivity_state_.Add(trajectory_id);
|
||||
data_.trajectory_connectivity_state.Add(trajectory_id);
|
||||
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
|
||||
CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0);
|
||||
frozen_trajectories_.insert(trajectory_id);
|
||||
CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0);
|
||||
data_.frozen_trajectories.insert(trajectory_id);
|
||||
});
|
||||
}
|
||||
|
||||
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(
|
||||
|
|
@ -466,13 +479,13 @@ void PoseGraph2D::AddSubmapFromProto(
|
|||
|
||||
common::MutexLocker locker(&mutex_);
|
||||
AddTrajectoryIfNeeded(submap_id.trajectory_id);
|
||||
submap_data_.Insert(submap_id, InternalSubmapData());
|
||||
submap_data_.at(submap_id).submap = submap_ptr;
|
||||
data_.submap_data.Insert(submap_id, InternalSubmapData());
|
||||
data_.submap_data.at(submap_id).submap = submap_ptr;
|
||||
// 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});
|
||||
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);
|
||||
});
|
||||
}
|
||||
|
|
@ -486,10 +499,12 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
|
|||
|
||||
common::MutexLocker locker(&mutex_);
|
||||
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_) {
|
||||
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(
|
||||
constant_data->gravity_alignment.inverse());
|
||||
optimization_problem_->InsertTrajectoryNode(
|
||||
|
|
@ -512,7 +527,7 @@ void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
|
|||
const SubmapId& submap_id) {
|
||||
common::MutexLocker locker(&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_);
|
||||
AddWorkItem([this, constraints]() REQUIRES(mutex_) {
|
||||
for (const auto& constraint : constraints) {
|
||||
CHECK(trajectory_nodes_.Contains(constraint.node_id));
|
||||
CHECK(submap_data_.Contains(constraint.submap_id));
|
||||
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
|
||||
CHECK(submap_data_.at(constraint.submap_id).submap != nullptr);
|
||||
CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
|
||||
CHECK(data_.submap_data.Contains(constraint.submap_id));
|
||||
CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data !=
|
||||
nullptr);
|
||||
CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr);
|
||||
switch (constraint.tag) {
|
||||
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)
|
||||
.second);
|
||||
break;
|
||||
|
|
@ -538,10 +554,10 @@ void PoseGraph2D::AddSerializedConstraints(
|
|||
const Constraint::Pose pose = {
|
||||
constraint.pose.zbar_ij *
|
||||
transform::Rigid3d::Rotation(
|
||||
trajectory_nodes_.at(constraint.node_id)
|
||||
data_.trajectory_nodes.at(constraint.node_id)
|
||||
.constant_data->gravity_alignment.inverse()),
|
||||
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});
|
||||
}
|
||||
LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
|
||||
|
|
@ -579,19 +595,19 @@ void PoseGraph2D::RunOptimization() {
|
|||
return;
|
||||
}
|
||||
|
||||
// No other thread is accessing the optimization_problem_, constraints_,
|
||||
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
|
||||
// time consuming, so not taking the mutex before Solve to avoid blocking
|
||||
// foreground processing.
|
||||
optimization_problem_->Solve(constraints_, frozen_trajectories_,
|
||||
landmark_nodes_);
|
||||
// No other thread is accessing the optimization_problem_,
|
||||
// data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
|
||||
// when executing the Solve. Solve is time consuming, so not taking the mutex
|
||||
// before Solve to avoid blocking foreground processing.
|
||||
optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories,
|
||||
data_.landmark_nodes);
|
||||
common::MutexLocker locker(&mutex_);
|
||||
|
||||
const auto& submap_data = optimization_problem_->submap_data();
|
||||
const auto& node_data = optimization_problem_->node_data();
|
||||
for (const int trajectory_id : node_data.trajectory_ids()) {
|
||||
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 =
|
||||
transform::Embed3D(node.data.global_pose_2d) *
|
||||
transform::Rigid3d::Rotation(
|
||||
|
|
@ -602,37 +618,38 @@ void PoseGraph2D::RunOptimization() {
|
|||
// 'optimization_problem_' yet.
|
||||
const auto local_to_new_global =
|
||||
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
|
||||
const auto local_to_old_global =
|
||||
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
|
||||
const auto local_to_old_global = ComputeLocalToGlobalTransform(
|
||||
data_.global_submap_poses_2d, trajectory_id);
|
||||
const transform::Rigid3d old_global_to_new_global =
|
||||
local_to_new_global * local_to_old_global.inverse();
|
||||
|
||||
const NodeId last_optimized_node_id =
|
||||
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
|
||||
auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
|
||||
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
|
||||
auto node_it =
|
||||
std::next(data_.trajectory_nodes.find(last_optimized_node_id));
|
||||
for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
|
||||
++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 =
|
||||
old_global_to_new_global * mutable_trajectory_node.global_pose;
|
||||
}
|
||||
}
|
||||
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 {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return trajectory_nodes_;
|
||||
return data_.trajectory_nodes;
|
||||
}
|
||||
|
||||
MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
|
||||
const {
|
||||
MapById<NodeId, TrajectoryNodePose> node_poses;
|
||||
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;
|
||||
if (node_id_data.data.constant_data != nullptr) {
|
||||
constant_pose_data = TrajectoryNodePose::ConstantPoseData{
|
||||
|
|
@ -650,7 +667,7 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
|
|||
const {
|
||||
std::map<std::string, transform::Rigid3d> landmark_poses;
|
||||
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.
|
||||
if (!landmark.second.global_landmark_pose.has_value()) continue;
|
||||
landmark_poses[landmark.first] =
|
||||
|
|
@ -663,7 +680,7 @@ void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
|
|||
const transform::Rigid3d& global_pose) {
|
||||
common::MutexLocker locker(&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>
|
||||
PoseGraph2D::GetLandmarkNodes() const {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return landmark_nodes_;
|
||||
return data_.landmark_nodes;
|
||||
}
|
||||
|
||||
std::map<int, PoseGraphInterface::TrajectoryData>
|
||||
|
|
@ -696,12 +713,12 @@ PoseGraph2D::GetFixedFramePoseData() const {
|
|||
std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const {
|
||||
std::vector<PoseGraphInterface::Constraint> result;
|
||||
common::MutexLocker locker(&mutex_);
|
||||
for (const Constraint& constraint : constraints_) {
|
||||
for (const Constraint& constraint : data_.constraints) {
|
||||
result.push_back(Constraint{
|
||||
constraint.submap_id, constraint.node_id,
|
||||
Constraint::Pose{constraint.pose.zbar_ij *
|
||||
transform::Rigid3d::Rotation(
|
||||
trajectory_nodes_.at(constraint.node_id)
|
||||
data_.trajectory_nodes.at(constraint.node_id)
|
||||
.constant_data->gravity_alignment),
|
||||
constraint.pose.translation_weight,
|
||||
constraint.pose.rotation_weight},
|
||||
|
|
@ -715,19 +732,20 @@ void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
|
|||
const transform::Rigid3d& pose,
|
||||
const common::Time time) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
initial_trajectory_poses_[from_trajectory_id] =
|
||||
data_.initial_trajectory_poses[from_trajectory_id] =
|
||||
InitialTrajectoryPose{to_trajectory_id, pose, time};
|
||||
}
|
||||
|
||||
transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
|
||||
const int trajectory_id, const common::Time time) const {
|
||||
CHECK_GT(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id), 0);
|
||||
const auto it = trajectory_nodes_.lower_bound(trajectory_id, time);
|
||||
if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) {
|
||||
return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose;
|
||||
CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0);
|
||||
const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time);
|
||||
if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) {
|
||||
return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)
|
||||
->data.global_pose;
|
||||
}
|
||||
if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) {
|
||||
return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id))
|
||||
if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) {
|
||||
return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
|
||||
->data.global_pose;
|
||||
}
|
||||
return transform::Interpolate(
|
||||
|
|
@ -742,11 +760,12 @@ transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
|
|||
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
|
||||
const int trajectory_id) const {
|
||||
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 {
|
||||
return trajectory_connectivity_state_.Components();
|
||||
return data_.trajectory_connectivity_state.Components();
|
||||
}
|
||||
|
||||
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(
|
||||
|
|
@ -765,7 +784,7 @@ MapById<SubmapId, PoseGraphInterface::SubmapPose>
|
|||
PoseGraph2D::GetAllSubmapPoses() const {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
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);
|
||||
submap_poses.Insert(
|
||||
submap_id_data.id,
|
||||
|
|
@ -781,8 +800,8 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
|
|||
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
|
||||
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
|
||||
if (begin_it == end_it) {
|
||||
const auto it = initial_trajectory_poses_.find(trajectory_id);
|
||||
if (it != initial_trajectory_poses_.end()) {
|
||||
const auto it = data_.initial_trajectory_poses.find(trajectory_id);
|
||||
if (it != data_.initial_trajectory_poses.end()) {
|
||||
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
|
||||
it->second.time) *
|
||||
it->second.relative_pose;
|
||||
|
|
@ -794,25 +813,26 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
|
|||
// Accessing 'local_pose' in Submap is okay, since the member is const.
|
||||
return transform::Embed3D(
|
||||
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()
|
||||
.inverse();
|
||||
}
|
||||
|
||||
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
|
||||
const SubmapId& submap_id) const {
|
||||
const auto it = submap_data_.find(submap_id);
|
||||
if (it == submap_data_.end()) {
|
||||
const auto it = data_.submap_data.find(submap_id);
|
||||
if (it == data_.submap_data.end()) {
|
||||
return {};
|
||||
}
|
||||
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.
|
||||
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.
|
||||
return {submap, ComputeLocalToGlobalTransform(global_submap_poses_,
|
||||
return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
|
||||
submap_id.trajectory_id) *
|
||||
submap->local_pose()};
|
||||
}
|
||||
|
|
@ -828,16 +848,17 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
|
|||
MapById<SubmapId, PoseGraphInterface::SubmapData>
|
||||
PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const {
|
||||
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 ||
|
||||
!parent_->global_submap_poses_.Contains(submap_id_data.id)) {
|
||||
!parent_->data_.global_submap_poses_2d.Contains(submap_id_data.id)) {
|
||||
continue;
|
||||
}
|
||||
submaps.Insert(submap_id_data.id,
|
||||
SubmapData{submap_id_data.data.submap,
|
||||
transform::Embed3D(parent_->global_submap_poses_
|
||||
.at(submap_id_data.id)
|
||||
.global_pose)});
|
||||
submaps.Insert(
|
||||
submap_id_data.id,
|
||||
SubmapData{submap_id_data.data.submap,
|
||||
transform::Embed3D(parent_->data_.global_submap_poses_2d
|
||||
.at(submap_id_data.id)
|
||||
.global_pose)});
|
||||
}
|
||||
return submaps;
|
||||
}
|
||||
|
|
@ -854,12 +875,12 @@ std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
|
|||
|
||||
const MapById<NodeId, TrajectoryNode>&
|
||||
PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {
|
||||
return parent_->trajectory_nodes_;
|
||||
return parent_->data_.trajectory_nodes;
|
||||
}
|
||||
|
||||
const std::vector<PoseGraphInterface::Constraint>&
|
||||
PoseGraph2D::TrimmingHandle::GetConstraints() const {
|
||||
return parent_->constraints_;
|
||||
return parent_->data_.constraints;
|
||||
}
|
||||
|
||||
bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {
|
||||
|
|
@ -870,22 +891,23 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
|
|||
const SubmapId& submap_id) {
|
||||
// TODO(hrapp): We have to make sure that the trajectory has been finished
|
||||
// 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
|
||||
// with 'submap_id' is gone.
|
||||
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 &&
|
||||
constraint.submap_id != submap_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::vector<Constraint> constraints;
|
||||
for (const Constraint& constraint : parent_->constraints_) {
|
||||
for (const Constraint& constraint : parent_->data_.constraints) {
|
||||
if (constraint.submap_id == submap_id) {
|
||||
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
|
||||
nodes_to_retain.count(constraint.node_id) == 0) {
|
||||
|
|
@ -897,29 +919,30 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
|
|||
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;
|
||||
for (const Constraint& constraint : parent_->constraints_) {
|
||||
for (const Constraint& constraint : parent_->data_.constraints) {
|
||||
if (nodes_to_remove.count(constraint.node_id) == 0) {
|
||||
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.
|
||||
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
|
||||
parent_->submap_data_.Trim(submap_id);
|
||||
CHECK(parent_->data_.submap_data.at(submap_id).state ==
|
||||
SubmapState::kFinished);
|
||||
parent_->data_.submap_data.Trim(submap_id);
|
||||
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
|
||||
parent_->optimization_problem_->TrimSubmap(submap_id);
|
||||
|
||||
// Remove the 'nodes_to_remove' from the pose graph and the optimization
|
||||
// problem.
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
@ -927,7 +950,7 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
|
|||
MapById<SubmapId, PoseGraphInterface::SubmapData>
|
||||
PoseGraph2D::GetSubmapDataUnderLock() const {
|
||||
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,
|
||||
GetSubmapDataUnderLock(submap_id_data.id));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@
|
|||
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
|
||||
#include "cartographer/mapping/internal/trajectory_connectivity_state.h"
|
||||
#include "cartographer/mapping/pose_graph.h"
|
||||
#include "cartographer/mapping/pose_graph_data.h"
|
||||
#include "cartographer/mapping/pose_graph_trimmer.h"
|
||||
#include "cartographer/sensor/fixed_frame_pose_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_);
|
||||
|
||||
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()
|
||||
const REQUIRES(mutex_);
|
||||
|
||||
|
|
@ -229,9 +215,6 @@ class PoseGraph2D : public PoseGraph {
|
|||
std::unique_ptr<std::deque<std::function<void()>>> work_queue_
|
||||
GUARDED_BY(mutex_);
|
||||
|
||||
// How our various trajectories are related.
|
||||
TrajectoryConnectivityState trajectory_connectivity_state_;
|
||||
|
||||
// We globally localize a fraction of the nodes from each trajectory.
|
||||
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
|
||||
global_localization_samplers_ GUARDED_BY(mutex_);
|
||||
|
|
@ -248,36 +231,11 @@ class PoseGraph2D : public PoseGraph {
|
|||
// Current optimization problem.
|
||||
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
|
||||
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.
|
||||
std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);
|
||||
|
||||
// Set of all frozen trajectories not being optimized.
|
||||
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_);
|
||||
PoseGraphData data_ GUARDED_BY(mutex_);
|
||||
|
||||
// Allows querying and manipulating the pose graph by the 'trimmers_'. The
|
||||
// 'mutex_' of the pose graph is held while this class is used.
|
||||
|
|
|
|||
|
|
@ -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_
|
||||
Loading…
Reference in New Issue