From af22dc6fe1551c7f4a89c65494e3a711cb8dbafd Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 21 Apr 2017 14:12:08 +0200 Subject: [PATCH] Simplify submap versions. (#235) We now only keep track of the number of inserted range data. --- cartographer/mapping/map_builder.cc | 3 +- cartographer/mapping/submaps.h | 13 ++--- cartographer/mapping_2d/submaps.cc | 17 +++--- cartographer/mapping_2d/submaps.h | 9 +--- cartographer/mapping_2d/submaps_test.cc | 10 +--- .../mapping_3d/global_trajectory_builder.cc | 7 ++- .../kalman_local_trajectory_builder.cc | 27 +++++----- .../kalman_local_trajectory_builder.h | 12 +++-- .../kalman_local_trajectory_builder_test.cc | 3 +- .../local_trajectory_builder_interface.h | 6 +-- .../optimizing_local_trajectory_builder.cc | 30 ++++++----- .../optimizing_local_trajectory_builder.h | 15 +++--- cartographer/mapping_3d/sparse_pose_graph.cc | 53 +++++++++++-------- cartographer/mapping_3d/sparse_pose_graph.h | 19 ++++--- cartographer/mapping_3d/submaps.cc | 30 ++++------- cartographer/mapping_3d/submaps.h | 15 ++---- 16 files changed, 121 insertions(+), 148 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 1743a44..4c68b24 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -141,8 +141,7 @@ string MapBuilder::SubmapToProto(const int trajectory_id, " submaps in this trajectory."; } - response->set_submap_version( - submaps->Get(submap_index)->end_range_data_index); + response->set_submap_version(submaps->Get(submap_index)->num_range_data); const std::vector submap_transforms = sparse_pose_graph_->GetSubmapTransforms(*submaps); CHECK_EQ(submap_transforms.size(), submaps->size()); diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index 1857fb2..7ad2879 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -64,14 +64,11 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) { } // An individual submap, which has an initial position 'origin', keeps track of -// which range data were inserted into it, and sets the +// how many range data were inserted into it, and sets the // 'finished_probability_grid' to be used for loop closing once the map no // longer changes. struct Submap { - Submap(const Eigen::Vector3f& origin, int begin_range_data_index) - : origin(origin), - begin_range_data_index(begin_range_data_index), - end_range_data_index(begin_range_data_index) {} + Submap(const Eigen::Vector3f& origin) : origin(origin) {} transform::Rigid3d local_pose() const { return transform::Rigid3d::Translation(origin.cast()); @@ -80,10 +77,8 @@ struct Submap { // Origin of this submap. Eigen::Vector3f origin; - // This Submap contains RangeData with indices in the range - // ['begin_range_data_index', 'end_range_data_index'). - int begin_range_data_index; - int end_range_data_index; + // Number of RangeData inserted. + int num_range_data = 0; // The 'finished_probability_grid' when this submap is finished and will not // change anymore. Otherwise, this is nullptr and the next call to diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index a221940..a05b782 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -101,10 +101,8 @@ proto::SubmapsOptions CreateSubmapsOptions( return options; } -Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin, - const int begin_range_data_index) - : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.), - begin_range_data_index), +Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin) + : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.)), probability_grid(limits) {} Submaps::Submaps(const proto::SubmapsOptions& options) @@ -116,16 +114,14 @@ Submaps::Submaps(const proto::SubmapsOptions& options) } void Submaps::InsertRangeData(const sensor::RangeData& range_data) { - CHECK_LT(num_range_data_, std::numeric_limits::max()); - ++num_range_data_; for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); CHECK(submap->finished_probability_grid == nullptr); range_data_inserter_.Insert(range_data, &submap->probability_grid); - submap->end_range_data_index = num_range_data_; + ++submap->num_range_data; } - ++num_range_data_in_last_submap_; - if (num_range_data_in_last_submap_ == options_.num_range_data()) { + const Submap* const last_submap = Get(size() - 1); + if (last_submap->num_range_data == options_.num_range_data()) { AddSubmap(range_data.origin.head<2>()); } } @@ -173,9 +169,8 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) { origin.cast() + options_.half_length() * Eigen::Vector2d::Ones(), CellLimits(num_cells_per_dimension, num_cells_per_dimension)), - origin, num_range_data_)); + origin)); LOG(INFO) << "Added submap " << size(); - num_range_data_in_last_submap_ = 0; } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index e119b71..30771c6 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -42,8 +42,7 @@ proto::SubmapsOptions CreateSubmapsOptions( common::LuaParameterDictionary* parameter_dictionary); struct Submap : public mapping::Submap { - Submap(const MapLimits& limits, const Eigen::Vector2f& origin, - int begin_range_data_index); + Submap(const MapLimits& limits, const Eigen::Vector2f& origin); ProbabilityGrid probability_grid; }; @@ -74,12 +73,6 @@ class Submaps : public mapping::Submaps { std::vector> submaps_; RangeDataInserter range_data_inserter_; - - // Number of RangeData inserted. - int num_range_data_ = 0; - - // Number of RangeData inserted since the last Submap was added. - int num_range_data_in_last_submap_ = 0; }; } // namespace mapping_2d diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index c818ae0..d56c87b 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -46,23 +46,17 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { "}," "}"); Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; - auto num_inserted = [&submaps](const int i) { - return submaps.Get(i)->end_range_data_index - - submaps.Get(i)->begin_range_data_index; - }; 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, num_inserted(matching)); + EXPECT_LE(kNumRangeData, submaps.Get(matching)->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, num_inserted(i)); - EXPECT_EQ(i * kNumRangeData, submaps.Get(i)->begin_range_data_index); - EXPECT_EQ((i + 2) * kNumRangeData, submaps.Get(i)->end_range_data_index); + EXPECT_EQ(kNumRangeData * 2, submaps.Get(i)->num_range_data); } } diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 39d6e18..1736d49 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -45,19 +45,18 @@ void GlobalTrajectoryBuilder::AddImuData( void GlobalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { - auto insertion_result = - local_trajectory_builder_->AddRangefinderData(time, origin, ranges); + auto insertion_result = local_trajectory_builder_->AddRangefinderData( + time, origin, ranges, sparse_pose_graph_->GetNextTrajectoryNodeIndex()); if (insertion_result == nullptr) { return; } - const int trajectory_node_index = sparse_pose_graph_->AddScan( + sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->range_data_in_tracking, insertion_result->pose_observation, insertion_result->covariance_estimate, insertion_result->submaps, insertion_result->matching_submap, insertion_result->insertion_submaps); - local_trajectory_builder_->AddTrajectoryNodeIndex(trajectory_node_index); } void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 5e5bbcf..8ce9ff7 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -72,7 +72,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData( std::unique_ptr KalmanLocalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) { + const sensor::PointCloud& ranges, int next_trajectory_node_index) { if (!pose_tracker_) { LOG(INFO) << "PoseTracker not yet initialized."; return nullptr; @@ -114,15 +114,18 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData( if (num_accumulated_ >= options_.scans_per_accumulation()) { num_accumulated_ = 0; return AddAccumulatedRangeData( - time, sensor::TransformRangeData(accumulated_range_data_, - tracking_delta.inverse())); + time, + sensor::TransformRangeData(accumulated_range_data_, + tracking_delta.inverse()), + next_trajectory_node_index); } return nullptr; } std::unique_ptr KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( - const common::Time time, const sensor::RangeData& range_data_in_tracking) { + const common::Time time, const sensor::RangeData& range_data_in_tracking, + const int trajectory_node_index) { const sensor::RangeData filtered_range_data = { range_data_in_tracking.origin, sensor::VoxelFiltered(range_data_in_tracking.returns, @@ -181,7 +184,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( pose_observation.cast())}; return InsertIntoSubmap(time, filtered_range_data, pose_observation, - covariance_estimate); + covariance_estimate, trajectory_node_index); } void KalmanLocalTrajectoryBuilder::AddOdometerData( @@ -206,16 +209,12 @@ KalmanLocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } -void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex( - int trajectory_node_index) { - submaps_->AddTrajectoryNodeIndex(trajectory_node_index); -} - std::unique_ptr KalmanLocalTrajectoryBuilder::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation, - const kalman_filter::PoseCovariance& covariance_estimate) { + const kalman_filter::PoseCovariance& covariance_estimate, + const int trajectory_node_index) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; } @@ -225,8 +224,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertRangeData(sensor::TransformRangeData( - range_data_in_tracking, pose_observation.cast())); + submaps_->InsertRangeData( + sensor::TransformRangeData(range_data_in_tracking, + pose_observation.cast()), + trajectory_node_index); return std::unique_ptr(new InsertionResult{ time, range_data_in_tracking, pose_observation, covariance_estimate, submaps_.get(), matching_submap, insertion_submaps}); diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index 34d7291..cf646df 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -50,21 +50,23 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { const Eigen::Vector3d& angular_velocity) override; std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) override; + const sensor::PointCloud& ranges, + int next_trajectory_node_index) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; - void AddTrajectoryNodeIndex(int trajectory_node_index) override; const mapping_3d::Submaps* submaps() const override; const PoseEstimate& pose_estimate() const override; private: std::unique_ptr AddAccumulatedRangeData( - common::Time time, const sensor::RangeData& range_data_in_tracking); + common::Time time, const sensor::RangeData& range_data_in_tracking, + int trajectory_node_index); std::unique_ptr InsertIntoSubmap( - const common::Time time, const sensor::RangeData& range_data_in_tracking, + common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation, - const kalman_filter::PoseCovariance& covariance_estimate); + const kalman_filter::PoseCovariance& covariance_estimate, + int trajectory_node_index); const proto::LocalTrajectoryBuilderOptions options_; std::unique_ptr submaps_; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index b3963a1..452d1a9 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -261,7 +261,8 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { AddLinearOnlyImuObservation(node.time, node.pose); const auto range_data = GenerateRangeData(node.pose); if (local_trajectory_builder_->AddRangefinderData( - node.time, range_data.origin, range_data.returns) != nullptr) { + node.time, range_data.origin, range_data.returns, num_poses) != + nullptr) { const auto pose_estimate = local_trajectory_builder_->pose_estimate(); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); ++num_poses; diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index 8410bf2..85b9a2e 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -55,14 +55,10 @@ class LocalTrajectoryBuilderInterface { const Eigen::Vector3d& angular_velocity) = 0; virtual std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) = 0; + const sensor::PointCloud& ranges, int next_trajectory_node_index) = 0; virtual void AddOdometerData(common::Time time, const transform::Rigid3d& pose) = 0; - // Register a 'trajectory_node_index' from the SparsePoseGraph corresponding - // to the latest inserted laser scan. This is used to remember which - // trajectory node should be used to visualize a Submap. - virtual void AddTrajectoryNodeIndex(int trajectory_node_index) = 0; virtual const mapping_3d::Submaps* submaps() const = 0; virtual const PoseEstimate& pose_estimate() const = 0; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index f4f8362..d44ef85 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -135,7 +135,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerData( std::unique_ptr OptimizingLocalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) { + const sensor::PointCloud& ranges, const int next_trajectory_node_index) { CHECK_GT(ranges.size(), 0); // TODO(hrapp): Handle misses. @@ -188,7 +188,7 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData( ++num_accumulated_; RemoveObsoleteSensorData(); - return MaybeOptimize(time); + return MaybeOptimize(time, next_trajectory_node_index); } void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() { @@ -215,7 +215,8 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() { } std::unique_ptr -OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { +OptimizingLocalTrajectoryBuilder::MaybeOptimize( + const common::Time time, const int trajectory_node_index) { // TODO(hrapp): Make the number of optimizations configurable. if (num_accumulated_ < options_.scans_per_accumulation() && num_accumulated_ % 10 != 0) { @@ -348,13 +349,15 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { } return AddAccumulatedRangeData(time, optimized_pose, - accumulated_range_data_in_tracking); + accumulated_range_data_in_tracking, + trajectory_node_index); } std::unique_ptr OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const transform::Rigid3d& optimized_pose, - const sensor::RangeData& range_data_in_tracking) { + const sensor::RangeData& range_data_in_tracking, + const int trajectory_node_index) { const sensor::RangeData filtered_range_data = { range_data_in_tracking.origin, sensor::VoxelFiltered(range_data_in_tracking.returns, @@ -372,7 +375,8 @@ OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData( sensor::TransformPointCloud(filtered_range_data.returns, optimized_pose.cast())}; - return InsertIntoSubmap(time, filtered_range_data, optimized_pose); + return InsertIntoSubmap(time, filtered_range_data, optimized_pose, + trajectory_node_index); } const OptimizingLocalTrajectoryBuilder::PoseEstimate& @@ -380,15 +384,11 @@ OptimizingLocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } -void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex( - int trajectory_node_index) { - submaps_->AddTrajectoryNodeIndex(trajectory_node_index); -} - std::unique_ptr OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose_observation) { + const transform::Rigid3d& pose_observation, + const int trajectory_node_index) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; } @@ -398,8 +398,10 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } - submaps_->InsertRangeData(sensor::TransformRangeData( - range_data_in_tracking, pose_observation.cast())); + submaps_->InsertRangeData( + sensor::TransformRangeData(range_data_in_tracking, + pose_observation.cast()), + trajectory_node_index); const kalman_filter::PoseCovariance kCovariance = 1e-7 * kalman_filter::PoseCovariance::Identity(); diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 7b90bc2..813b759 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -53,10 +53,10 @@ class OptimizingLocalTrajectoryBuilder const Eigen::Vector3d& angular_velocity) override; std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) override; - void AddOdometerData(const common::Time time, + const sensor::PointCloud& ranges, + int next_trajectory_node_index) override; + void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; - void AddTrajectoryNodeIndex(int trajectory_node_index) override; const mapping_3d::Submaps* submaps() const override; const PoseEstimate& pose_estimate() const override; @@ -102,13 +102,16 @@ class OptimizingLocalTrajectoryBuilder std::unique_ptr AddAccumulatedRangeData( common::Time time, const transform::Rigid3d& pose_observation, - const sensor::RangeData& range_data_in_tracking); + const sensor::RangeData& range_data_in_tracking, + const int next_trajectory_node_index); std::unique_ptr InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose_observation); + const transform::Rigid3d& pose_observation, + const int next_trajectory_node_index); - std::unique_ptr MaybeOptimize(common::Time time); + std::unique_ptr MaybeOptimize( + common::Time time, const int next_trajectory_node_index); const proto::LocalTrajectoryBuilderOptions options_; const ceres::Solver::Options ceres_solver_options_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 13e9b4b..36da741 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -83,7 +83,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( } } -int SparsePoseGraph::AddScan( +void SparsePoseGraph::AddScan( common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance, const Submaps* submaps, @@ -126,7 +126,11 @@ int SparsePoseGraph::AddScan( ComputeConstraintsForScan(j, matching_submap, insertion_submaps, finished_submap, pose, covariance); }); - return j; +} + +int SparsePoseGraph::GetNextTrajectoryNodeIndex() { + common::MutexLocker locker(&mutex_); + return trajectory_nodes_.size(); } void SparsePoseGraph::AddWorkItem(std::function work_item) { @@ -285,25 +289,30 @@ void SparsePoseGraph::WaitForAllComputations() { common::MutexLocker locker(&mutex_); const int num_finished_scans_at_start = constraint_builder_.GetNumFinishedScans(); - while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) { - return static_cast(constraint_builder_.GetNumFinishedScans()) == - trajectory_nodes_.size(); - }, common::FromSeconds(1.))) { + while (!locker.AwaitWithTimeout( + [this]() REQUIRES(mutex_) { + return static_cast(constraint_builder_.GetNumFinishedScans()) == + trajectory_nodes_.size(); + }, + common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) - << 100. * (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / + << 100. * + (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / (trajectory_nodes_.size() - - num_finished_scans_at_start) << "%..."; + num_finished_scans_at_start) + << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; - constraint_builder_.WhenDone([this, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { - constraints_.insert(constraints_.end(), result.begin(), result.end()); - common::MutexLocker locker(&mutex_); - notification = true; - }); + constraint_builder_.WhenDone( + [this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + common::MutexLocker locker(&mutex_); + notification = true; + }); locker.Await([¬ification]() { return notification; }); } @@ -336,13 +345,13 @@ void SparsePoseGraph::RunOptimization() { const mapping::Submaps* trajectory = trajectory_nodes_[i].constant_data->trajectory; if (extrapolation_transforms.count(trajectory) == 0) { - extrapolation_transforms[trajectory] = - transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_, - *trajectory).back() * - ExtrapolateSubmapTransforms( - optimized_submap_transforms_, *trajectory) - .back() - .inverse()); + extrapolation_transforms[trajectory] = transform::Rigid3d( + ExtrapolateSubmapTransforms(submap_transforms_, *trajectory) + .back() * + ExtrapolateSubmapTransforms(optimized_submap_transforms_, + *trajectory) + .back() + .inverse()); } trajectory_nodes_[i].pose = extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 995b7ab..99c3f5b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -69,16 +69,19 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose' // that will later be optimized. The 'pose' was determined by scan matching // against the 'matching_submap' and the scan was inserted into the - // 'insertion_submaps'. The index into the vector of trajectory nodes as - // used with GetTrajectoryNodes() is returned. - int AddScan(common::Time time, - const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& pose_covariance, - const Submaps* submaps, const Submap* matching_submap, - const std::vector& insertion_submaps) + // 'insertion_submaps'. + void AddScan(common::Time time, + const sensor::RangeData& range_data_in_tracking, + const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& pose_covariance, + const Submaps* submaps, const Submap* matching_submap, + const std::vector& insertion_submaps) EXCLUDES(mutex_); + // The index into the vector of trajectory nodes as used with + // GetTrajectoryNodes() for the next node added with AddScan() is returned. + int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_); + // Adds new IMU data to be used in the optimization. void AddImuData(const mapping::Submaps* trajectory, common::Time time, const Eigen::Vector3d& linear_acceleration, diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index d047a85..9231000 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -217,8 +217,8 @@ proto::SubmapsOptions CreateSubmapsOptions( } Submap::Submap(const float high_resolution, const float low_resolution, - const Eigen::Vector3f& origin, const int begin_range_data_index) - : mapping::Submap(origin, begin_range_data_index), + const Eigen::Vector3f& origin) + : mapping::Submap(origin), high_resolution_hybrid_grid(high_resolution, origin), low_resolution_hybrid_grid(low_resolution, origin) {} @@ -274,9 +274,8 @@ void Submaps::SubmapToProto( global_submap_pose.translation().z()))); } -void Submaps::InsertRangeData(const sensor::RangeData& range_data) { - CHECK_LT(num_range_data_, std::numeric_limits::max()); - ++num_range_data_; +void Submaps::InsertRangeData(const sensor::RangeData& range_data, + const int trajectory_node_index) { for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); range_data_inserter_.Insert( @@ -285,10 +284,11 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) { &submap->high_resolution_hybrid_grid); range_data_inserter_.Insert(range_data, &submap->low_resolution_hybrid_grid); - submap->end_range_data_index = num_range_data_; + ++submap->num_range_data; + submap->trajectory_node_indices.push_back(trajectory_node_index); } - ++num_range_data_in_last_submap_; - if (num_range_data_in_last_submap_ == options_.num_range_data()) { + const Submap* const last_submap = Get(size() - 1); + if (last_submap->num_range_data == options_.num_range_data()) { AddSubmap(range_data.origin); } } @@ -301,16 +301,6 @@ const HybridGrid& Submaps::low_resolution_matching_grid() const { return submaps_[matching_index()]->low_resolution_hybrid_grid; } -void Submaps::AddTrajectoryNodeIndex(const int trajectory_node_index) { - for (int i = 0; i != size(); ++i) { - Submap& submap = *submaps_[i]; - if (submap.end_range_data_index == num_range_data_ && - submap.begin_range_data_index <= num_range_data_ - 1) { - submap.trajectory_node_indices.push_back(trajectory_node_index); - } - } -} - void Submaps::AddSubmap(const Eigen::Vector3f& origin) { if (size() > 1) { Submap* submap = submaps_[size() - 2].get(); @@ -318,10 +308,8 @@ void Submaps::AddSubmap(const Eigen::Vector3f& origin) { submap->finished = true; } submaps_.emplace_back(new Submap(options_.high_resolution(), - options_.low_resolution(), origin, - num_range_data_)); + options_.low_resolution(), origin)); LOG(INFO) << "Added submap " << size(); - num_range_data_in_last_submap_ = 0; } std::vector Submaps::AccumulatePixelData( diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 6b90727..17ced54 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -47,11 +47,12 @@ proto::SubmapsOptions CreateSubmapsOptions( struct Submap : public mapping::Submap { Submap(float high_resolution, float low_resolution, - const Eigen::Vector3f& origin, int begin_range_data_index); + const Eigen::Vector3f& origin); HybridGrid high_resolution_hybrid_grid; HybridGrid low_resolution_hybrid_grid; bool finished = false; + // Indices into the nodes of the SparsePoseGraph used to visualize the submap. std::vector trajectory_node_indices; }; @@ -71,7 +72,8 @@ class Submaps : public mapping::Submaps { mapping::proto::SubmapQuery::Response* response) const override; // Inserts 'range_data' into the Submap collection. - void InsertRangeData(const sensor::RangeData& range_data); + void InsertRangeData(const sensor::RangeData& range_data, + int trajectory_node_index); // Returns the 'high_resolution' HybridGrid to be used for matching. const HybridGrid& high_resolution_matching_grid() const; @@ -79,9 +81,6 @@ class Submaps : public mapping::Submaps { // Returns the 'low_resolution' HybridGrid to be used for matching. const HybridGrid& low_resolution_matching_grid() const; - // Adds a node to be used when visualizing the submap. - void AddTrajectoryNodeIndex(int trajectory_node_index); - private: struct PixelData { int min_z = INT_MAX; @@ -112,12 +111,6 @@ class Submaps : public mapping::Submaps { std::vector> submaps_; RangeDataInserter range_data_inserter_; - - // Number of RangeData inserted. - int num_range_data_ = 0; - - // Number of RangeData inserted since the last Submap was added. - int num_range_data_in_last_submap_ = 0; }; } // namespace mapping_3d