diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index dcb6697..38a5915 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -17,7 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ #define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ -#include +#include #include #include "Eigen/Core" @@ -31,7 +31,7 @@ namespace mapping { class Submaps; struct TrajectoryNode { - struct ConstantData { + struct Data { common::Time time; // Range data in 'pose' frame. Only used in the 2D case. @@ -51,7 +51,9 @@ struct TrajectoryNode { common::Time time() const { return constant_data->time; } - const ConstantData* constant_data; + // This must be a shared_ptr. If the data is used for visualization while the + // node is being deleted, it must survive until all use finishes. + std::shared_ptr constant_data; transform::Rigid3d pose; }; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index e5ee08f..cc8a4da 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -97,14 +97,15 @@ void SparsePoseGraph::AddScan( GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); common::MutexLocker locker(&mutex_); - constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ - time, range_data_in_pose, - Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), - trajectory_id, tracking_to_pose}); - trajectory_nodes_.Append(trajectory_id, - mapping::TrajectoryNode{ - &constant_node_data_.back(), optimized_pose, - }); + trajectory_nodes_.Append( + trajectory_id, + mapping::TrajectoryNode{ + std::make_shared( + mapping::TrajectoryNode::Data{ + time, range_data_in_pose, + Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), + trajectory_id, tracking_to_pose}), + optimized_pose}); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); @@ -224,8 +225,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( .at(matching_id.trajectory_id) .size()) : 0}; - const mapping::TrajectoryNode::ConstantData* const scan_data = - trajectory_nodes_.at(node_id).constant_data; + const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, scan_data->time, pose, optimized_pose); @@ -466,8 +466,8 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( } else if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } else { - // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps - // is okay, since the member is const. + // Extrapolate to the remaining submaps. Accessing 'local_pose' in + // Submaps is okay, since the member is const. const mapping::SubmapId previous_submap_id{ trajectory_id, static_cast(result.size()) - 1}; result.push_back( @@ -493,8 +493,8 @@ int SparsePoseGraph::TrimmingHandle::num_submaps( void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( const mapping::SubmapId& submap_id) { - // TODO(hrapp): We have to make sure that the trajectory has been finished if - // we want to delete the last submaps. + // 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); // Compile all nodes that are still INTRA_SUBMAP constrained once the submap @@ -538,7 +538,6 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( // Mark the submap with 'submap_id' as trimmed and remove its data. parent_->submap_data_.at(submap_id).state = SubmapState::kTrimmed; parent_->constraint_builder_.DeleteScanMatcher(submap_id); - // TODO(hrapp): Make 'Submap' object thread safe and remove submap data in // there. @@ -548,8 +547,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( // TODO(whess): The optimization problem should no longer include the submap // and the removed nodes. - // TODO(whess): If the first submap is gone, we want to tie the first not yet - // trimmed submap to be set fixed to its current pose. + // TODO(whess): If the first submap is gone, we want to tie the first not + // yet trimmed submap to be set fixed to its current pose. } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index f381cad..3988a7f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -194,10 +194,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::map reverse_connected_components_; // Data that are currently being shown. - // - // Deque to keep references valid for the background computation when adding - // new data. - std::deque constant_node_data_; mapping::NestedVectorsById trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index c216715..23dc2e6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -95,13 +95,15 @@ void SparsePoseGraph::AddScan( GetLocalToGlobalTransform(trajectory_id) * pose); common::MutexLocker locker(&mutex_); - constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ - time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, - sensor::Compress(range_data_in_tracking), trajectory_id, - transform::Rigid3d::Identity()}); trajectory_nodes_.Append( trajectory_id, - mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); + mapping::TrajectoryNode{ + std::make_shared( + mapping::TrajectoryNode::Data{ + time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, + sensor::Compress(range_data_in_tracking), trajectory_id, + transform::Rigid3d::Identity()}), + optimized_pose}); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); @@ -240,8 +242,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( .at(matching_id.trajectory_id) .size()) : 0}; - const mapping::TrajectoryNode::ConstantData* const scan_data = - trajectory_nodes_.at(node_id).constant_data; + const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, scan_data->time, optimized_pose); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 9291fb9..47a4abc 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -191,10 +191,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::map reverse_connected_components_; // Data that are currently being shown. - // - // Deque to keep references valid for the background computation when adding - // new data. - std::deque constant_node_data_; mapping::NestedVectorsById trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;