Use PoseGraphData in PoseGraph3d. (#1188)
[RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0022-pose-graph-data.md)master
Alexander Belyaev
GitHub
parent
3437b931dd
commit
5055703490
|
|
@ -61,26 +61,28 @@ std::vector<SubmapId> PoseGraph3D::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,
|
||||
ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) *
|
||||
insertion_submaps[0]->local_pose());
|
||||
trajectory_id, ComputeLocalToGlobalTransform(
|
||||
data_.global_submap_poses_3d, 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;
|
||||
|
|
@ -91,10 +93,12 @@ std::vector<SubmapId> PoseGraph3D::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};
|
||||
}
|
||||
|
||||
|
|
@ -107,19 +111,19 @@ NodeId PoseGraph3D::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
|
||||
|
|
@ -141,7 +145,7 @@ void PoseGraph3D::AddWorkItem(const std::function<void()>& work_item) {
|
|||
}
|
||||
|
||||
void PoseGraph3D::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] =
|
||||
|
|
@ -182,7 +186,7 @@ void PoseGraph3D::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(
|
||||
PoseGraphInterface::LandmarkNode::LandmarkObservation{
|
||||
trajectory_id, landmark_data.time,
|
||||
observation.landmark_to_tracking_transform,
|
||||
|
|
@ -193,7 +197,7 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id,
|
|||
|
||||
void PoseGraph3D::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 transform::Rigid3d global_node_pose =
|
||||
optimization_problem_->node_data().at(node_id).global_pose;
|
||||
|
|
@ -205,16 +209,17 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
|
|||
global_submap_pose.inverse();
|
||||
|
||||
std::vector<TrajectoryNode> submap_nodes;
|
||||
for (const NodeId& submap_node_id : submap_data_.at(submap_id).node_ids) {
|
||||
submap_nodes.push_back(
|
||||
TrajectoryNode{trajectory_nodes_.at(submap_node_id).constant_data,
|
||||
global_submap_pose_inverse *
|
||||
trajectory_nodes_.at(submap_node_id).global_pose});
|
||||
for (const NodeId& submap_node_id :
|
||||
data_.submap_data.at(submap_id).node_ids) {
|
||||
submap_nodes.push_back(TrajectoryNode{
|
||||
data_.trajectory_nodes.at(submap_node_id).constant_data,
|
||||
global_submap_pose_inverse *
|
||||
data_.trajectory_nodes.at(submap_node_id).global_pose});
|
||||
}
|
||||
|
||||
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 <
|
||||
|
|
@ -226,9 +231,11 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
|
|||
// submap's trajectory, it suffices to do a match constrained to a local
|
||||
// search window.
|
||||
constraint_builder_.MaybeAddConstraint(
|
||||
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
|
||||
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
|
||||
global_node_pose, global_submap_pose);
|
||||
submap_id,
|
||||
static_cast<const Submap3D*>(
|
||||
data_.submap_data.at(submap_id).submap.get()),
|
||||
node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
|
||||
submap_nodes, global_node_pose, global_submap_pose);
|
||||
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
|
||||
// In this situation, 'global_node_pose' and 'global_submap_pose' have
|
||||
// orientations agreeing on gravity. Their relationship regarding yaw is
|
||||
|
|
@ -236,14 +243,17 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
|
|||
// matching procedure in the FastCorrelativeScanMatcher, and the given yaw
|
||||
// is essentially ignored.
|
||||
constraint_builder_.MaybeAddGlobalConstraint(
|
||||
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
|
||||
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
|
||||
global_node_pose.rotation(), global_submap_pose.rotation());
|
||||
submap_id,
|
||||
static_cast<const Submap3D*>(
|
||||
data_.submap_data.at(submap_id).submap.get()),
|
||||
node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
|
||||
submap_nodes, global_node_pose.rotation(),
|
||||
global_submap_pose.rotation());
|
||||
}
|
||||
}
|
||||
|
||||
void PoseGraph3D::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) {
|
||||
|
|
@ -256,7 +266,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
|
|||
const NodeId& node_id,
|
||||
std::vector<std::shared_ptr<const Submap3D>> 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());
|
||||
|
|
@ -271,12 +281,12 @@ void PoseGraph3D::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);
|
||||
// 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::Rigid3d constraint_transform =
|
||||
insertion_submaps[i]->local_pose().inverse() * local_pose;
|
||||
constraints_.push_back(
|
||||
data_.constraints.push_back(
|
||||
Constraint{submap_id,
|
||||
node_id,
|
||||
{constraint_transform, options_.matcher_translation_weight(),
|
||||
|
|
@ -284,7 +294,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
|
|||
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);
|
||||
|
|
@ -294,7 +304,7 @@ void PoseGraph3D::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
|
||||
|
|
@ -322,13 +332,14 @@ void PoseGraph3D::DispatchOptimization() {
|
|||
|
||||
common::Time PoseGraph3D::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;
|
||||
}
|
||||
|
|
@ -337,16 +348,17 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
|
|||
CHECK_EQ(constraint.tag, PoseGraphInterface::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 PoseGraph3D::HandleWorkQueue(
|
||||
const constraints::ConstraintBuilder3D::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();
|
||||
|
||||
|
|
@ -408,18 +420,19 @@ void PoseGraph3D::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;
|
||||
}
|
||||
|
|
@ -429,7 +442,8 @@ void PoseGraph3D::WaitForAllComputations() {
|
|||
[this,
|
||||
¬ification](const constraints::ConstraintBuilder3D::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; });
|
||||
|
|
@ -438,11 +452,11 @@ void PoseGraph3D::WaitForAllComputations() {
|
|||
void PoseGraph3D::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();
|
||||
|
|
@ -450,20 +464,20 @@ void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
|
|||
}
|
||||
|
||||
bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const {
|
||||
return finished_trajectories_.count(trajectory_id) > 0;
|
||||
return data_.finished_trajectories.count(trajectory_id) > 0;
|
||||
}
|
||||
|
||||
void PoseGraph3D::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 PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const {
|
||||
return frozen_trajectories_.count(trajectory_id) > 0;
|
||||
return data_.frozen_trajectories.count(trajectory_id) > 0;
|
||||
}
|
||||
|
||||
void PoseGraph3D::AddSubmapFromProto(
|
||||
|
|
@ -479,13 +493,13 @@ void PoseGraph3D::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(submap_id,
|
||||
optimization::SubmapSpec3D{global_submap_pose});
|
||||
data_.global_submap_poses_3d.Insert(
|
||||
submap_id, optimization::SubmapSpec3D{global_submap_pose});
|
||||
AddWorkItem([this, submap_id, global_submap_pose]() 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);
|
||||
});
|
||||
}
|
||||
|
|
@ -499,10 +513,12 @@ void PoseGraph3D::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;
|
||||
optimization_problem_->InsertTrajectoryNode(
|
||||
node_id,
|
||||
optimization::NodeSpec3D{constant_data->time, constant_data->local_pose,
|
||||
|
|
@ -533,7 +549,7 @@ void PoseGraph3D::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);
|
||||
});
|
||||
}
|
||||
|
||||
|
|
@ -542,13 +558,14 @@ void PoseGraph3D::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;
|
||||
|
|
@ -556,7 +573,7 @@ void PoseGraph3D::AddSerializedConstraints(
|
|||
UpdateTrajectoryConnectivity(constraint);
|
||||
break;
|
||||
}
|
||||
constraints_.push_back(constraint);
|
||||
data_.constraints.push_back(constraint);
|
||||
}
|
||||
LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
|
||||
});
|
||||
|
|
@ -591,14 +608,14 @@ void PoseGraph3D::RunFinalOptimization() {
|
|||
void PoseGraph3D::LogResidualHistograms() const {
|
||||
common::Histogram rotational_residual;
|
||||
common::Histogram translational_residual;
|
||||
for (const Constraint& constraint : constraints_) {
|
||||
for (const Constraint& constraint : data_.constraints) {
|
||||
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
|
||||
const cartographer::transform::Rigid3d optimized_node_to_map =
|
||||
trajectory_nodes_.at(constraint.node_id).global_pose;
|
||||
data_.trajectory_nodes.at(constraint.node_id).global_pose;
|
||||
const cartographer::transform::Rigid3d node_to_submap_constraint =
|
||||
constraint.pose.zbar_ij;
|
||||
const cartographer::transform::Rigid3d optimized_submap_to_map =
|
||||
global_submap_poses_.at(constraint.submap_id).global_pose;
|
||||
data_.global_submap_poses_3d.at(constraint.submap_id).global_pose;
|
||||
const cartographer::transform::Rigid3d optimized_node_to_submap =
|
||||
optimized_submap_to_map.inverse() * optimized_node_to_map;
|
||||
const cartographer::transform::Rigid3d residual =
|
||||
|
|
@ -619,44 +636,45 @@ void PoseGraph3D::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)) {
|
||||
trajectory_nodes_.at(node.id).global_pose = node.data.global_pose;
|
||||
data_.trajectory_nodes.at(node.id).global_pose = node.data.global_pose;
|
||||
}
|
||||
|
||||
// Extrapolate all point cloud poses that were not included in the
|
||||
// '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_3d, 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_3d = submap_data;
|
||||
|
||||
// Log the histograms for the pose residuals.
|
||||
if (options_.log_residual_histograms()) {
|
||||
|
|
@ -666,14 +684,14 @@ void PoseGraph3D::RunOptimization() {
|
|||
|
||||
MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() const {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return trajectory_nodes_;
|
||||
return data_.trajectory_nodes;
|
||||
}
|
||||
|
||||
MapById<NodeId, TrajectoryNodePose> PoseGraph3D::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{
|
||||
|
|
@ -691,7 +709,7 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::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] =
|
||||
|
|
@ -704,7 +722,7 @@ void PoseGraph3D::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;
|
||||
});
|
||||
}
|
||||
|
||||
|
|
@ -727,7 +745,7 @@ PoseGraph3D::GetFixedFramePoseData() const {
|
|||
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
|
||||
PoseGraph3D::GetLandmarkNodes() const {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return landmark_nodes_;
|
||||
return data_.landmark_nodes;
|
||||
}
|
||||
|
||||
std::map<int, PoseGraphInterface::TrajectoryData>
|
||||
|
|
@ -738,7 +756,7 @@ PoseGraph3D::GetTrajectoryData() const {
|
|||
|
||||
std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() const {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return constraints_;
|
||||
return data_.constraints;
|
||||
}
|
||||
|
||||
void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id,
|
||||
|
|
@ -746,19 +764,20 @@ void PoseGraph3D::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 PoseGraph3D::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(
|
||||
|
|
@ -773,11 +792,12 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose(
|
|||
transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform(
|
||||
const int trajectory_id) const {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
|
||||
return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d,
|
||||
trajectory_id);
|
||||
}
|
||||
|
||||
std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() const {
|
||||
return trajectory_connectivity_state_.Components();
|
||||
return data_.trajectory_connectivity_state.Components();
|
||||
}
|
||||
|
||||
PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData(
|
||||
|
|
@ -796,7 +816,7 @@ MapById<SubmapId, PoseGraphInterface::SubmapPose>
|
|||
PoseGraph3D::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,
|
||||
|
|
@ -812,8 +832,8 @@ transform::Rigid3d PoseGraph3D::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;
|
||||
|
|
@ -824,24 +844,24 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
|
|||
const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
|
||||
// Accessing 'local_pose' in Submap is okay, since the member is const.
|
||||
return 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 PoseGraph3D::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_3d.Contains(submap_id)) {
|
||||
// We already have an optimized pose.
|
||||
return {submap, global_submap_poses_.at(submap_id).global_pose};
|
||||
return {submap, data_.global_submap_poses_3d.at(submap_id).global_pose};
|
||||
}
|
||||
// We have to extrapolate.
|
||||
return {submap, ComputeLocalToGlobalTransform(global_submap_poses_,
|
||||
return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_3d,
|
||||
submap_id.trajectory_id) *
|
||||
submap->local_pose()};
|
||||
}
|
||||
|
|
@ -866,28 +886,28 @@ std::vector<SubmapId> PoseGraph3D::TrimmingHandle::GetSubmapIds(
|
|||
MapById<SubmapId, PoseGraphInterface::SubmapData>
|
||||
PoseGraph3D::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_3d.Contains(submap_id_data.id)) {
|
||||
continue;
|
||||
}
|
||||
submaps.Insert(
|
||||
submap_id_data.id,
|
||||
SubmapData{
|
||||
submap_id_data.data.submap,
|
||||
parent_->global_submap_poses_.at(submap_id_data.id).global_pose});
|
||||
SubmapData{submap_id_data.data.submap,
|
||||
parent_->data_.global_submap_poses_3d.at(submap_id_data.id)
|
||||
.global_pose});
|
||||
}
|
||||
return submaps;
|
||||
}
|
||||
|
||||
const MapById<NodeId, TrajectoryNode>&
|
||||
PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const {
|
||||
return parent_->trajectory_nodes_;
|
||||
return parent_->data_.trajectory_nodes;
|
||||
}
|
||||
|
||||
const std::vector<PoseGraphInterface::Constraint>&
|
||||
PoseGraph3D::TrimmingHandle::GetConstraints() const {
|
||||
return parent_->constraints_;
|
||||
return parent_->data_.constraints;
|
||||
}
|
||||
|
||||
bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const {
|
||||
|
|
@ -898,22 +918,23 @@ void PoseGraph3D::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) {
|
||||
|
|
@ -925,29 +946,30 @@ void PoseGraph3D::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);
|
||||
}
|
||||
}
|
||||
|
|
@ -955,7 +977,7 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
|
|||
MapById<SubmapId, PoseGraphInterface::SubmapData>
|
||||
PoseGraph3D::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_3d.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"
|
||||
|
|
@ -150,21 +151,6 @@ class PoseGraph3D : public PoseGraph {
|
|||
void WaitForAllComputations() EXCLUDES(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 Submap3D> 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, SubmapData> GetSubmapDataUnderLock() const REQUIRES(mutex_);
|
||||
|
||||
// Handles a new work item.
|
||||
|
|
@ -235,9 +221,6 @@ class PoseGraph3D : 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_);
|
||||
|
|
@ -251,36 +234,11 @@ class PoseGraph3D : public PoseGraph {
|
|||
// Current optimization problem.
|
||||
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
|
||||
constraints::ConstraintBuilder3D 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::SubmapSpec3D> 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.
|
||||
|
|
|
|||
Loading…
Reference in New Issue