From 772d02c00ecc6c0910335aab35a5ba2bd80d630f Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Mon, 8 May 2017 17:10:07 +0200 Subject: [PATCH] Move ConstantData into the SPG (#263) --- cartographer/mapping/map_builder.cc | 8 +++----- cartographer/mapping/map_builder.h | 5 +---- cartographer/mapping_2d/sparse_pose_graph.cc | 10 ++++------ cartographer/mapping_2d/sparse_pose_graph.h | 8 +++----- cartographer/mapping_2d/sparse_pose_graph_test.cc | 3 +-- cartographer/mapping_3d/sparse_pose_graph.cc | 10 ++++------ cartographer/mapping_3d/sparse_pose_graph.h | 8 +++----- 7 files changed, 19 insertions(+), 33 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index eb03e2d..c8ce8c4 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -51,18 +51,16 @@ proto::MapBuilderOptions CreateMapBuilderOptions( return options; } -MapBuilder::MapBuilder( - const proto::MapBuilderOptions& options, - std::deque* const constant_data) +MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : options_(options), thread_pool_(options.num_background_threads()) { if (options.use_trajectory_builder_2d()) { sparse_pose_graph_2d_ = common::make_unique( - options_.sparse_pose_graph_options(), &thread_pool_, constant_data); + options_.sparse_pose_graph_options(), &thread_pool_); sparse_pose_graph_ = sparse_pose_graph_2d_.get(); } if (options.use_trajectory_builder_3d()) { sparse_pose_graph_3d_ = common::make_unique( - options_.sparse_pose_graph_options(), &thread_pool_, constant_data); + options_.sparse_pose_graph_options(), &thread_pool_); sparse_pose_graph_ = sparse_pose_graph_3d_.get(); } } diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 56fb5d5..35e61bb 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -17,7 +17,6 @@ #ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ #define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ -#include #include #include #include @@ -34,7 +33,6 @@ #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder.h" -#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_3d/sparse_pose_graph.h" #include "cartographer/sensor/collator.h" @@ -49,8 +47,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions( // and a SparsePoseGraph for loop closure. class MapBuilder { public: - MapBuilder(const proto::MapBuilderOptions& options, - std::deque* constant_data); + MapBuilder(const proto::MapBuilderOptions& options); ~MapBuilder(); MapBuilder(const MapBuilder&) = delete; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 3d87be3..0123454 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -40,12 +40,10 @@ namespace mapping_2d { SparsePoseGraph::SparsePoseGraph( const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool, - std::deque* constant_node_data) + common::ThreadPool* thread_pool) : options_(options), optimization_problem_(options_.optimization_problem_options()), - constraint_builder_(options_.constraint_builder_options(), thread_pool), - constant_node_data_(constant_node_data) {} + constraint_builder_(options_.constraint_builder_options(), thread_pool) {} SparsePoseGraph::~SparsePoseGraph() { WaitForAllComputations(); @@ -96,12 +94,12 @@ void SparsePoseGraph::AddScan( const int j = trajectory_nodes_.size(); CHECK_LT(j, std::numeric_limits::max()); - constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ + constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, range_data_in_pose, Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps, tracking_to_pose}); trajectory_nodes_.push_back(mapping::TrajectoryNode{ - &constant_node_data_->back(), optimized_pose, + &constant_node_data_.back(), optimized_pose, }); trajectory_connectivity_.Add(submaps); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 531542c..b8dd225 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -56,10 +56,8 @@ namespace mapping_2d { // Implements the SPA loop closure method. class SparsePoseGraph : public mapping::SparsePoseGraph { public: - SparsePoseGraph( - const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool, - std::deque* constant_node_data); + SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool); ~SparsePoseGraph() override; SparsePoseGraph(const SparsePoseGraph&) = delete; @@ -185,7 +183,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // // Deque to keep references valid for the background computation when adding // new data. - std::deque* constant_node_data_; + std::deque constant_node_data_; std::vector trajectory_nodes_ GUARDED_BY(mutex_); // Current submap transforms used for displaying data. diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 7c2ed15..3cb9d2f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -135,7 +135,7 @@ class SparsePoseGraphTest : public ::testing::Test { })text"); sparse_pose_graph_ = common::make_unique( mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), - &thread_pool_, &constant_node_data_); + &thread_pool_); } current_pose_ = transform::Rigid2d::Identity(); @@ -172,7 +172,6 @@ class SparsePoseGraphTest : public ::testing::Test { sensor::PointCloud point_cloud_; std::unique_ptr submaps_; - std::deque constant_node_data_; common::ThreadPool thread_pool_; std::unique_ptr sparse_pose_graph_; transform::Rigid2d current_pose_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 8f2b5a9..75133f4 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -40,13 +40,11 @@ namespace mapping_3d { SparsePoseGraph::SparsePoseGraph( const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool, - std::deque* constant_node_data) + common::ThreadPool* thread_pool) : options_(options), optimization_problem_(options_.optimization_problem_options(), sparse_pose_graph::OptimizationProblem::FixZ::kNo), - constraint_builder_(options_.constraint_builder_options(), thread_pool), - constant_node_data_(constant_node_data) {} + constraint_builder_(options_.constraint_builder_options(), thread_pool) {} SparsePoseGraph::~SparsePoseGraph() { WaitForAllComputations(); @@ -95,12 +93,12 @@ void SparsePoseGraph::AddScan( const int j = trajectory_nodes_.size(); CHECK_LT(j, std::numeric_limits::max()); - constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ + constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, sensor::Compress(range_data_in_tracking), submaps, transform::Rigid3d::Identity()}); trajectory_nodes_.push_back( - mapping::TrajectoryNode{&constant_node_data_->back(), optimized_pose}); + mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); trajectory_connectivity_.Add(submaps); if (submap_indices_.count(insertion_submaps.back()) == 0) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index b042cb1..6b83401 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -56,10 +56,8 @@ namespace mapping_3d { // Implements the SPA loop closure method. class SparsePoseGraph : public mapping::SparsePoseGraph { public: - SparsePoseGraph( - const mapping::proto::SparsePoseGraphOptions& options, - common::ThreadPool* thread_pool, - std::deque* constant_node_data); + SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool); ~SparsePoseGraph() override; SparsePoseGraph(const SparsePoseGraph&) = delete; @@ -205,7 +203,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // // Deque to keep references valid for the background computation when adding // new data. - std::deque* constant_node_data_; + std::deque constant_node_data_; std::vector trajectory_nodes_ GUARDED_BY(mutex_); // Current submap transforms used for displaying data.