From 58bb70f53a5eb625b272c2c4ff2910b768361542 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 20 Jun 2017 18:21:42 +0200 Subject: [PATCH] Change Submaps to only track the two active Submaps. (#348) --- .../mapping_2d/global_trajectory_builder.cc | 8 ++-- .../mapping_2d/local_trajectory_builder.cc | 19 ++++---- .../mapping_2d/local_trajectory_builder.h | 4 +- cartographer/mapping_2d/sparse_pose_graph.cc | 6 +++ cartographer/mapping_2d/sparse_pose_graph.h | 2 + .../mapping_2d/sparse_pose_graph_test.cc | 10 ++-- cartographer/mapping_2d/submaps.cc | 47 +++++++------------ cartographer/mapping_2d/submaps.h | 29 +++++------- cartographer/mapping_2d/submaps_test.cc | 23 ++++++--- .../mapping_3d/global_trajectory_builder.cc | 8 ++-- .../kalman_local_trajectory_builder.cc | 14 ++---- .../kalman_local_trajectory_builder.h | 3 +- .../local_trajectory_builder_interface.h | 1 - .../optimizing_local_trajectory_builder.cc | 14 ++---- .../optimizing_local_trajectory_builder.h | 3 +- cartographer/mapping_3d/sparse_pose_graph.cc | 6 +++ cartographer/mapping_3d/sparse_pose_graph.h | 2 + cartographer/mapping_3d/submaps.cc | 42 ++++++----------- cartographer/mapping_3d/submaps.h | 27 ++++------- 19 files changed, 120 insertions(+), 148 deletions(-) diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index 06cbfda..7fcf596 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -34,9 +34,11 @@ int GlobalTrajectoryBuilder::num_submaps() { GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData( const int submap_index) { - return {local_trajectory_builder_.submaps()->Get(submap_index), - sparse_pose_graph_->GetSubmapTransform( - mapping::SubmapId{trajectory_id_, submap_index})}; + // TODO(hrapp): Get rid of this function and query the sparse pose graph + // directly. + const mapping::SubmapId submap_id{trajectory_id_, submap_index}; + return {sparse_pose_graph_->GetSubmap(submap_id), + sparse_pose_graph_->GetSubmapTransform(submap_id)}; } void GlobalTrajectoryBuilder::AddRangefinderData( diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index e37005b..46a32c2 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -27,7 +27,7 @@ namespace mapping_2d { LocalTrajectoryBuilder::LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), - submaps_(options.submaps_options()), + active_submaps_(options.submaps_options()), motion_filter_(options_.motion_filter_options()), real_time_correlative_scan_matcher_( options_.real_time_correlative_scan_matcher_options()), @@ -36,8 +36,6 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder( LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} -Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; } - sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::RangeData& range_data) const { @@ -70,8 +68,8 @@ void LocalTrajectoryBuilder::ScanMatch( const transform::Rigid3d& tracking_to_tracking_2d, const sensor::RangeData& range_data_in_tracking_2d, transform::Rigid3d* pose_observation) { - const ProbabilityGrid& probability_grid = - submaps_.Get(submaps_.matching_index())->probability_grid(); + std::shared_ptr matching_submap = + active_submaps_.submaps().front(); transform::Rigid2d pose_prediction_2d = transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse()); // The online correlative scan matcher will refine the initial estimate for @@ -84,14 +82,15 @@ void LocalTrajectoryBuilder::ScanMatch( if (options_.use_online_correlative_scan_matching()) { real_time_correlative_scan_matcher_.Match( pose_prediction_2d, filtered_point_cloud_in_tracking_2d, - probability_grid, &initial_ceres_pose); + matching_submap->probability_grid(), &initial_ceres_pose); } transform::Rigid2d tracking_2d_to_map; ceres::Solver::Summary summary; ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose, filtered_point_cloud_in_tracking_2d, - probability_grid, &tracking_2d_to_map, &summary); + matching_submap->probability_grid(), + &tracking_2d_to_map, &summary); *pose_observation = transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d; @@ -177,10 +176,10 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( } std::vector> insertion_submaps; - for (int insertion_index : submaps_.insertion_indices()) { - insertion_submaps.push_back(submaps_.Get(insertion_index)); + for (std::shared_ptr submap : active_submaps_.submaps()) { + insertion_submaps.push_back(submap); } - submaps_.InsertRangeData( + active_submaps_.InsertRangeData( TransformRangeData(range_data_in_tracking_2d, transform::Embed3D(pose_estimate_2d.cast()))); diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index eefeff5..1f77b6e 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -62,8 +62,6 @@ class LocalTrajectoryBuilder { const Eigen::Vector3d& angular_velocity); void AddOdometerData(common::Time time, const transform::Rigid3d& pose); - Submaps* submaps(); - private: sensor::RangeData TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, @@ -83,7 +81,7 @@ class LocalTrajectoryBuilder { void Predict(common::Time time); const proto::LocalTrajectoryBuilderOptions options_; - Submaps submaps_; + ActiveSubmaps active_submaps_; mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_; // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'. diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 549b3f4..1acac5b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -424,6 +424,12 @@ SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_.data(); } +std::shared_ptr SparsePoseGraph::GetSubmap( + const mapping::SubmapId& submap_id) { + common::MutexLocker locker(&mutex_); + return submap_data_.at(submap_id).submap; +} + std::vector SparsePoseGraph::constraints() { common::MutexLocker locker(&mutex_); return constraints_; diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index ad74f71..ddb4094 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -90,6 +90,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); + std::shared_ptr GetSubmap(const mapping::SubmapId& submap_id) + EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); private: diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 701b4c9..6103cf6 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -60,7 +60,7 @@ class SparsePoseGraphTest : public ::testing::Test { miss_probability = 0.495, }, })text"); - submaps_ = common::make_unique( + active_submaps_ = common::make_unique( CreateSubmapsOptions(parameter_dictionary.get())); } @@ -150,14 +150,14 @@ class SparsePoseGraphTest : public ::testing::Test { point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); std::vector> insertion_submaps; - for (int insertion_index : submaps_->insertion_indices()) { - insertion_submaps.push_back(submaps_->Get(insertion_index)); + for (auto submap : active_submaps_->submaps()) { + insertion_submaps.push_back(submap); } const sensor::RangeData range_data{ Eigen::Vector3f::Zero(), new_point_cloud, {}}; const transform::Rigid2d pose_estimate = noise * current_pose_; constexpr int kTrajectoryId = 0; - submaps_->InsertRangeData(TransformRangeData( + active_submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); sparse_pose_graph_->AddScan( @@ -170,7 +170,7 @@ class SparsePoseGraphTest : public ::testing::Test { } sensor::PointCloud point_cloud_; - std::unique_ptr submaps_; + std::unique_ptr active_submaps_; common::ThreadPool thread_pool_; std::unique_ptr sparse_pose_graph_; transform::Rigid2d current_pose_; diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index 7800b9d..8becb1b 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -151,7 +151,7 @@ void Submap::ToResponseProto( transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); } -Submaps::Submaps(const proto::SubmapsOptions& options) +ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options) : options_(options), range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one likelihood field which we can return, @@ -159,9 +159,8 @@ Submaps::Submaps(const proto::SubmapsOptions& options) AddSubmap(Eigen::Vector2f::Zero()); } -void Submaps::InsertRangeData(const sensor::RangeData& range_data) { - for (const int index : insertion_indices()) { - Submap* submap = submaps_[index].get(); +void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) { + for (auto& submap : submaps_) { CHECK(!submap->finished_); range_data_inserter_.Insert(range_data, &submap->probability_grid_); ++submap->num_range_data_; @@ -171,46 +170,32 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) { } } -std::shared_ptr Submaps::Get(int index) const { - CHECK_GE(index, 0); - CHECK_LT(index, size()); - return submaps_[index]; +std::vector> ActiveSubmaps::submaps() const { + return submaps_; } -int Submaps::size() const { return submaps_.size(); } +int ActiveSubmaps::matching_index() const { return matching_submap_index_; } -int Submaps::matching_index() const { - if (size() > 1) { - return size() - 2; - } - return size() - 1; -} - -std::vector Submaps::insertion_indices() const { - if (size() > 1) { - return {size() - 2, size() - 1}; - } - return {size() - 1}; -} - -void Submaps::FinishSubmap(int index) { +void ActiveSubmaps::FinishSubmap() { // Crop the finished Submap before inserting a new Submap to reduce peak // memory usage a bit. - Submap* submap = submaps_[index].get(); + Submap* submap = submaps_.front().get(); CHECK(!submap->finished_); submap->probability_grid_ = ComputeCroppedProbabilityGrid(submap->probability_grid_); submap->finished_ = true; if (options_.output_debug_images()) { // Output the Submap that won't be changed from now on. - WriteDebugImage("submap" + std::to_string(index) + ".webp", - submap->probability_grid_); + WriteDebugImage("submap" + std::to_string(matching_submap_index_) + ".webp", + submaps_.front()->probability_grid_); } + ++matching_submap_index_; + submaps_.erase(submaps_.begin()); } -void Submaps::AddSubmap(const Eigen::Vector2f& origin) { - if (size() > 1) { - FinishSubmap(size() - 2); +void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) { + if (submaps_.size() > 1) { + FinishSubmap(); } const int num_cells_per_dimension = common::RoundToInt(2. * options_.half_length() / options_.resolution()) + @@ -221,7 +206,7 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) { options_.half_length() * Eigen::Vector2d::Ones(), CellLimits(num_cells_per_dimension, num_cells_per_dimension)), origin)); - LOG(INFO) << "Added submap " << size(); + LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index ba5bd62..f8f8e4b 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -54,15 +54,12 @@ class Submap : public mapping::Submap { private: // TODO(hrapp): Remove friend declaration. - friend class Submaps; + friend class ActiveSubmaps; ProbabilityGrid probability_grid_; bool finished_ = false; }; -// Submaps is a sequence of maps to which scans are matched and into which scans -// are inserted. -// // Except during initialization when only a single submap exists, there are // always two submaps into which scans are inserted: an old submap that is used // for matching, and a new one, which will be used for matching next, that is @@ -70,34 +67,30 @@ class Submap : public mapping::Submap { // // Once a certain number of scans have been inserted, the new submap is // considered initialized: the old submap is no longer changed, the "new" submap -// is now the "old" submap and is used for scan-to-map matching. Moreover, -// a "new" submap gets inserted. -class Submaps { +// is now the "old" submap and is used for scan-to-map matching. Moreover, a +// "new" submap gets created. The "old" submap is forgotten by this object. +class ActiveSubmaps { public: - explicit Submaps(const proto::SubmapsOptions& options); + explicit ActiveSubmaps(const proto::SubmapsOptions& options); - Submaps(const Submaps&) = delete; - Submaps& operator=(const Submaps&) = delete; - - std::shared_ptr Get(int index) const; - int size() const; + ActiveSubmaps(const ActiveSubmaps&) = delete; + ActiveSubmaps& operator=(const ActiveSubmaps&) = delete; // Returns the index of the newest initialized Submap which can be // used for scan-to-map matching. int matching_index() const; - // Returns the indices of the Submap into which point clouds will - // be inserted. - std::vector insertion_indices() const; - // Inserts 'range_data' into the Submap collection. void InsertRangeData(const sensor::RangeData& range_data); + std::vector> submaps() const; + private: - void FinishSubmap(int index); + void FinishSubmap(); void AddSubmap(const Eigen::Vector2f& origin); const proto::SubmapsOptions options_; + int matching_submap_index_ = 0; std::vector> submaps_; RangeDataInserter range_data_inserter_; }; diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index f70638f..1ce505a 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -17,6 +17,8 @@ #include "cartographer/mapping_2d/submaps.h" #include +#include +#include #include #include "cartographer/common/lua_parameter_dictionary.h" @@ -45,19 +47,26 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { "miss_probability = 0.495, " "}," "}"); - Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; + ActiveSubmaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; + std::set> all_submaps; for (int i = 0; i != 1000; ++i) { submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); - const int matching = submaps.matching_index(); // Except for the first, maps should only be returned after enough scans. - if (matching != 0) { - EXPECT_LE(kNumRangeData, submaps.Get(matching)->num_range_data()); + for (auto submap : submaps.submaps()) { + all_submaps.insert(submap); + } + if (submaps.matching_index() != 0) { + EXPECT_LE(kNumRangeData, submaps.submaps().front()->num_range_data()); } } - for (int i = 0; i != submaps.size() - 2; ++i) { - // Submaps should not be left without the right number of scans in them. - EXPECT_EQ(kNumRangeData * 2, submaps.Get(i)->num_range_data()); + int correct_num_scans = 0; + for (const auto& submap : all_submaps) { + if (submap->num_range_data() == kNumRangeData * 2) { + ++correct_num_scans; + } } + // Submaps should not be left without the right number of scans in them. + EXPECT_EQ(correct_num_scans, all_submaps.size() - 2); } } // namespace diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index aabcee6..095d171 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -36,9 +36,11 @@ int GlobalTrajectoryBuilder::num_submaps() { GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData( const int submap_index) { - return {local_trajectory_builder_->submaps()->Get(submap_index), - sparse_pose_graph_->GetSubmapTransform( - mapping::SubmapId{trajectory_id_, submap_index})}; + // TODO(hrapp): Get rid of this function and query the sparse pose graph + // directly. + const mapping::SubmapId submap_id{trajectory_id_, submap_index}; + return {sparse_pose_graph_->GetSubmap(submap_id), + sparse_pose_graph_->GetSubmapTransform(submap_id)}; } void GlobalTrajectoryBuilder::AddImuData( diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 4775964..caf9f49 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -31,7 +31,7 @@ namespace mapping_3d { KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), - submaps_(common::make_unique(options.submaps_options())), + active_submaps_(options.submaps_options()), scan_matcher_pose_estimate_(transform::Rigid3d::Identity()), motion_filter_(options.motion_filter_options()), real_time_correlative_scan_matcher_( @@ -46,10 +46,6 @@ KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder( KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {} -mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() { - return submaps_.get(); -} - void KalmanLocalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { @@ -141,7 +137,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( time, &pose_prediction, &unused_covariance_prediction); std::shared_ptr matching_submap = - submaps_->Get(submaps_->matching_index()); + active_submaps_.submaps().front(); transform::Rigid3d initial_ceres_pose = matching_submap->local_pose().inverse() * pose_prediction; sensor::AdaptiveVoxelFilter adaptive_voxel_filter( @@ -220,10 +216,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( return nullptr; } std::vector> insertion_submaps; - for (int insertion_index : submaps_->insertion_indices()) { - insertion_submaps.push_back(submaps_->Get(insertion_index)); + for (std::shared_ptr submap : active_submaps_.submaps()) { + insertion_submaps.push_back(submap); } - submaps_->InsertRangeData( + active_submaps_.InsertRangeData( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), pose_tracker_->gravity_orientation()); diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index ab45993..1c933ea 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -53,7 +53,6 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { const sensor::PointCloud& ranges) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; - mapping_3d::Submaps* submaps() override; const PoseEstimate& pose_estimate() const override; private: @@ -65,7 +64,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { const transform::Rigid3d& pose_observation); const proto::LocalTrajectoryBuilderOptions options_; - std::unique_ptr submaps_; + mapping_3d::ActiveSubmaps active_submaps_; PoseEstimate last_pose_estimate_; diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index 2e116d1..f8db3b5 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -56,7 +56,6 @@ class LocalTrajectoryBuilderInterface { virtual void AddOdometerData(common::Time time, const transform::Rigid3d& pose) = 0; - virtual mapping_3d::Submaps* submaps() = 0; virtual const PoseEstimate& pose_estimate() const = 0; protected: diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index b1bfd29..29300bf 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -106,16 +106,12 @@ OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder( : options_(options), ceres_solver_options_(common::CreateCeresSolverOptions( options.ceres_scan_matcher_options().ceres_solver_options())), - submaps_(common::make_unique(options.submaps_options())), + active_submaps_(options.submaps_options()), num_accumulated_(0), motion_filter_(options.motion_filter_options()) {} OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {} -mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() { - return submaps_.get(); -} - void OptimizingLocalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { @@ -237,7 +233,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { ceres::Problem problem; std::shared_ptr matching_submap = - submaps_->Get(submaps_->matching_index()); + active_submaps_.submaps().front(); // We transform the states in 'batches_' in place to be in the submap frame as // expected by the OccupiedSpaceCostFunctor. This is reverted after solving // the optimization problem. @@ -410,13 +406,13 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( return nullptr; } std::vector> insertion_submaps; - for (int insertion_index : submaps_->insertion_indices()) { - insertion_submaps.push_back(submaps_->Get(insertion_index)); + for (std::shared_ptr submap : active_submaps_.submaps()) { + insertion_submaps.push_back(submap); } // TODO(whess): Use an ImuTracker to track the gravity direction. const Eigen::Quaterniond kFakeGravityOrientation = Eigen::Quaterniond::Identity(); - submaps_->InsertRangeData( + active_submaps_.InsertRangeData( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), kFakeGravityOrientation); diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 318611d..2e2c7ec 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -56,7 +56,6 @@ class OptimizingLocalTrajectoryBuilder const sensor::PointCloud& ranges) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; - mapping_3d::Submaps* submaps() override; const PoseEstimate& pose_estimate() const override; private: @@ -116,7 +115,7 @@ class OptimizingLocalTrajectoryBuilder const proto::LocalTrajectoryBuilderOptions options_; const ceres::Solver::Options ceres_solver_options_; - std::unique_ptr submaps_; + mapping_3d::ActiveSubmaps active_submaps_; int num_accumulated_; std::deque batches_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 2595e89..c215d19 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -424,6 +424,12 @@ SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_.data(); } +std::shared_ptr SparsePoseGraph::GetSubmap( + const mapping::SubmapId& submap_id) { + common::MutexLocker locker(&mutex_); + return submap_data_.at(submap_id).submap; +} + std::vector SparsePoseGraph::constraints() { common::MutexLocker locker(&mutex_); return constraints_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 8c1b217..ad03cf8 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -89,6 +89,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); + std::shared_ptr GetSubmap(const mapping::SubmapId& submap_id) + EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); private: diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index b17a799..b3731d0 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -360,7 +360,7 @@ void Submap::ToResponseProto( global_submap_pose.translation().z()))); } -Submaps::Submaps(const proto::SubmapsOptions& options) +ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options) : options_(options), range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one submap which we can return and will @@ -371,32 +371,16 @@ Submaps::Submaps(const proto::SubmapsOptions& options) AddSubmap(transform::Rigid3d::Identity()); } -std::shared_ptr Submaps::Get(int index) const { - CHECK_GE(index, 0); - CHECK_LT(index, size()); - return submaps_[index]; +std::vector> ActiveSubmaps::submaps() const { + return submaps_; } -int Submaps::size() const { return submaps_.size(); } +int ActiveSubmaps::matching_index() const { return matching_submap_index_; } -int Submaps::matching_index() const { - if (size() > 1) { - return size() - 2; - } - return size() - 1; -} - -std::vector Submaps::insertion_indices() const { - if (size() > 1) { - return {size() - 2, size() - 1}; - } - return {size() - 1}; -} - -void Submaps::InsertRangeData(const sensor::RangeData& range_data, - const Eigen::Quaterniond& gravity_alignment) { - for (const int index : insertion_indices()) { - Submap* submap = submaps_[index].get(); +void ActiveSubmaps::InsertRangeData( + const sensor::RangeData& range_data, + const Eigen::Quaterniond& gravity_alignment) { + for (auto& submap : submaps_) { const sensor::RangeData transformed_range_data = sensor::TransformRangeData( range_data, submap->local_pose().inverse().cast()); range_data_inserter_.Insert( @@ -413,15 +397,17 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data, } } -void Submaps::AddSubmap(const transform::Rigid3d& local_pose) { - if (size() > 1) { - Submap* submap = submaps_[size() - 2].get(); +void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_pose) { + if (submaps_.size() > 1) { + Submap* submap = submaps_.front().get(); CHECK(!submap->finished_); submap->finished_ = true; + ++matching_submap_index_; + submaps_.erase(submaps_.begin()); } submaps_.emplace_back(new Submap(options_.high_resolution(), options_.low_resolution(), local_pose)); - LOG(INFO) << "Added submap " << size(); + LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 5096720..13f3f00 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -66,16 +66,13 @@ class Submap : public mapping::Submap { private: // TODO(hrapp): Remove friend declaration. - friend class Submaps; + friend class ActiveSubmaps; HybridGrid high_resolution_hybrid_grid_; HybridGrid low_resolution_hybrid_grid_; bool finished_ = false; }; -// Submaps is a sequence of maps to which scans are matched and into which scans -// are inserted. -// // Except during initialization when only a single submap exists, there are // always two submaps into which scans are inserted: an old submap that is used // for matching, and a new one, which will be used for matching next, that is @@ -83,36 +80,32 @@ class Submap : public mapping::Submap { // // Once a certain number of scans have been inserted, the new submap is // considered initialized: the old submap is no longer changed, the "new" submap -// is now the "old" submap and is used for scan-to-map matching. Moreover, -// a "new" submap gets inserted. -class Submaps { +// is now the "old" submap and is used for scan-to-map matching. Moreover, a +// "new" submap gets created. The "old" submap is forgotten by this object. +class ActiveSubmaps { public: - explicit Submaps(const proto::SubmapsOptions& options); + explicit ActiveSubmaps(const proto::SubmapsOptions& options); - Submaps(const Submaps&) = delete; - Submaps& operator=(const Submaps&) = delete; - - std::shared_ptr Get(int index) const; - int size() const; + ActiveSubmaps(const ActiveSubmaps&) = delete; + ActiveSubmaps& operator=(const ActiveSubmaps&) = delete; // Returns the index of the newest initialized Submap which can be // used for scan-to-map matching. int matching_index() const; - // Returns the indices of the Submap into which point clouds will - // be inserted. - std::vector insertion_indices() const; - // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is // used for the orientation of new submaps so that the z axis approximately // aligns with gravity. void InsertRangeData(const sensor::RangeData& range_data, const Eigen::Quaterniond& gravity_alignment); + std::vector> submaps() const; + private: void AddSubmap(const transform::Rigid3d& local_pose); const proto::SubmapsOptions options_; + int matching_submap_index_ = 0; std::vector> submaps_; RangeDataInserter range_data_inserter_; };