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

Remove trimmed() from TrajectoryNode (#596)

master
Christoph Schütte 2017-10-17 15:36:07 +02:00 committed by Wolfgang Hess
parent e5f9815f67
commit 3d4650d675
4 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/sparse_pose_graph.cc
View File

@ -79,7 +79,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
for (size_t old_node_index = 0; for (size_t old_node_index = 0;
old_node_index != single_trajectory_nodes.size(); ++old_node_index) { old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
const auto& node = single_trajectory_nodes[old_node_index]; const auto& node = single_trajectory_nodes[old_node_index];
if (!node.trimmed()) { if (node.constant_data != nullptr) {
node_id_remapping[NodeId{static_cast<int>(trajectory_id), node_id_remapping[NodeId{static_cast<int>(trajectory_id),
static_cast<int>(old_node_index)}] = static_cast<int>(old_node_index)}] =
NodeId{static_cast<int>(trajectory_id), NodeId{static_cast<int>(trajectory_id),

1
cartographer/mapping/trajectory_node.h
View File

@ -51,7 +51,6 @@ struct TrajectoryNode {
}; };
common::Time time() const { return constant_data->time; } common::Time time() const { return constant_data->time; }
bool trimmed() const { return constant_data == nullptr; }
// This must be a shared_ptr. If the data is used for visualization while the // This must be a shared_ptr. If the data is used for visualization while the
// node is being trimmed, it must survive until all use finishes. // node is being trimmed, it must survive until all use finishes.

7
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -202,7 +202,6 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_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 mapping::NodeId& node_id = node_id_data.id; const mapping::NodeId& node_id = node_id_data.id;
CHECK(!trajectory_nodes_.at(node_id).trimmed());
if (submap_data.node_ids.count(node_id) == 0) { if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id); ComputeConstraint(node_id, submap_id);
} }
@ -649,10 +648,10 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id);
// Mark the 'nodes_to_remove' as trimmed and remove their data. // Remove the 'nodes_to_remove' from the sparse pose graph and the
// optimization problem.
for (const mapping::NodeId& node_id : nodes_to_remove) { for (const mapping::NodeId& node_id : nodes_to_remove) {
CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed()); parent_->trajectory_nodes_.Trim(node_id);
parent_->trajectory_nodes_.at(node_id).constant_data.reset();
parent_->optimization_problem_.TrimTrajectoryNode(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id);
} }
} }

7
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -228,7 +228,6 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_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 mapping::NodeId& node_id = node_id_data.id; const mapping::NodeId& node_id = node_id_data.id;
CHECK(!trajectory_nodes_.at(node_id).trimmed());
if (submap_data.node_ids.count(node_id) == 0) { if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id); ComputeConstraint(node_id, submap_id);
} }
@ -676,10 +675,10 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id);
// Mark the 'nodes_to_remove' as trimmed and remove their data. // Remove the 'nodes_to_remove' from the sparse pose graph and the
// optimization problem.
for (const mapping::NodeId& node_id : nodes_to_remove) { for (const mapping::NodeId& node_id : nodes_to_remove) {
CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed()); parent_->trajectory_nodes_.Trim(node_id);
parent_->trajectory_nodes_.at(node_id).constant_data.reset();
parent_->optimization_problem_.TrimTrajectoryNode(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id);
} }
} }