diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index 20794d5..25d2cc5 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -67,13 +67,6 @@ class Submap { // Number of RangeData inserted. size_t num_range_data() const { return num_range_data_; } - // The 'finished_probability_grid' when this submap is finished and will not - // change anymore. Otherwise, this is nullptr and the next call to - // InsertRangeData() will change the submap. - const mapping_2d::ProbabilityGrid* finished_probability_grid() const { - return finished_probability_grid_; - } - // Fills data into the 'response'. virtual void ToResponseProto( const transform::Rigid3d& global_submap_pose, @@ -83,9 +76,8 @@ class Submap { const transform::Rigid3d local_pose_; protected: - // TODO(hrapp): All of this should be private. + // TODO(hrapp): This should be private. int num_range_data_ = 0; - const mapping_2d::ProbabilityGrid* finished_probability_grid_ = nullptr; }; // Submaps is a sequence of maps to which scans are matched and into which scans diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index fcd6c54..21db9e4 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -45,13 +45,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData( std::unique_ptr insertion_result = local_trajectory_builder_.AddHorizontalRangeData( time, sensor::RangeData{origin, ranges, {}}); - if (insertion_result != nullptr) { - sparse_pose_graph_->AddScan( - insertion_result->time, insertion_result->tracking_to_tracking_2d, - insertion_result->range_data_in_tracking_2d, - insertion_result->pose_estimate_2d, trajectory_id_, - insertion_result->matching_submap, insertion_result->insertion_submaps); + if (insertion_result == nullptr) { + return; } + const Submap* const finished_submap = + insertion_result->insertion_submaps.front()->finished() + ? insertion_result->insertion_submaps.front() + : nullptr; + sparse_pose_graph_->AddScan( + insertion_result->time, insertion_result->tracking_to_tracking_2d, + insertion_result->range_data_in_tracking_2d, + insertion_result->pose_estimate_2d, trajectory_id_, + insertion_result->matching_submap, insertion_result->insertion_submaps, + finished_submap); } void GlobalTrajectoryBuilder::AddImuData( diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index b76715b..cf44097 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -176,9 +176,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( return nullptr; } - const mapping::Submap* const matching_submap = - submaps_.Get(submaps_.matching_index()); - std::vector insertion_submaps; + const Submap* const matching_submap = submaps_.Get(submaps_.matching_index()); + std::vector insertion_submaps; for (int insertion_index : submaps_.insertion_indices()) { insertion_submaps.push_back(submaps_.Get(insertion_index)); } diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index b953d52..13440de 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -41,8 +41,8 @@ class LocalTrajectoryBuilder { public: struct InsertionResult { common::Time time; - const mapping::Submap* matching_submap; - std::vector insertion_submaps; + const Submap* matching_submap; + std::vector insertion_submaps; transform::Rigid3d tracking_to_tracking_2d; sensor::RangeData range_data_in_tracking_2d; transform::Rigid2d pose_estimate_2d; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 0d88ecf..d665102 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -53,7 +53,7 @@ SparsePoseGraph::~SparsePoseGraph() { } void SparsePoseGraph::GrowSubmapTransformsAsNeeded( - const std::vector& insertion_submaps) { + const std::vector& insertion_submaps) { CHECK(!insertion_submaps.empty()); const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]); const int trajectory_id = first_submap_id.trajectory_id; @@ -91,8 +91,9 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, - const int trajectory_id, const mapping::Submap* const matching_submap, - const std::vector& insertion_submaps) { + const int trajectory_id, const Submap* const matching_submap, + const std::vector& insertion_submaps, + const Submap* const finished_submap) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); @@ -115,10 +116,6 @@ void SparsePoseGraph::AddScan( submap_ids_.emplace(insertion_submaps.back(), submap_id); submap_data_.at(submap_id).submap = insertion_submaps.back(); } - const mapping::Submap* const finished_submap = - insertion_submaps.front()->finished_probability_grid() != nullptr - ? insertion_submaps.front() - : nullptr; // Make sure we have a sampler for this trajectory. if (!global_localization_samplers_[trajectory_id]) { @@ -188,8 +185,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, } } -void SparsePoseGraph::ComputeConstraintsForOldScans( - const mapping::Submap* submap) { +void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { const auto submap_id = GetSubmapId(submap); const auto& submap_data = submap_data_.at(submap_id); @@ -209,9 +205,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( } void SparsePoseGraph::ComputeConstraintsForScan( - const mapping::Submap* matching_submap, - std::vector insertion_submaps, - const mapping::Submap* finished_submap, const transform::Rigid2d& pose) { + const Submap* matching_submap, std::vector insertion_submaps, + const Submap* finished_submap, const transform::Rigid2d& pose) { GrowSubmapTransformsAsNeeded(insertion_submaps); const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid2d optimized_pose = @@ -231,7 +226,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, scan_data->time, pose, optimized_pose); - for (const mapping::Submap* submap : insertion_submaps) { + for (const Submap* submap : insertion_submaps) { const mapping::SubmapId submap_id = GetSubmapId(submap); CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 9f4a4c4..78a0c59 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -66,13 +66,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // that will later be optimized. The 'tracking_to_pose' is remembered so // that the optimized pose can be embedded into 3D. The 'pose' was determined // by scan matching against the 'matching_submap' and the scan was inserted - // into the 'insertion_submaps'. + // into the 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a + // freshly finished submap. It is also contained in 'insertion_submaps' for + // the last time. void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, int trajectory_id, - const mapping::Submap* matching_submap, - const std::vector& insertion_submaps) - EXCLUDES(mutex_); + const Submap* matching_submap, + const std::vector& insertion_submaps, + const Submap* finished_submap) EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. void AddImuData(int trajectory_id, common::Time time, @@ -97,7 +99,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Likewise, all new scans are matched against submaps which are finished. enum class SubmapState { kActive, kFinished, kTrimmed }; struct SubmapData { - const mapping::Submap* submap = nullptr; + const Submap* submap = nullptr; // IDs of the scans that were inserted into this map together with // constraints for them. They are not to be matched again when this submap @@ -110,8 +112,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Handles a new work item. void AddWorkItem(std::function work_item) REQUIRES(mutex_); - mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const - REQUIRES(mutex_) { + mapping::SubmapId GetSubmapId(const Submap* submap) const REQUIRES(mutex_) { const auto iterator = submap_ids_.find(submap); CHECK(iterator != submap_ids_.end()); return iterator->second; @@ -120,14 +121,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. void GrowSubmapTransformsAsNeeded( - const std::vector& insertion_submaps) - REQUIRES(mutex_); + const std::vector& insertion_submaps) REQUIRES(mutex_); // Adds constraints for a scan, and starts scan matching in the background. - void ComputeConstraintsForScan( - const mapping::Submap* matching_submap, - std::vector insertion_submaps, - const mapping::Submap* finished_submap, const transform::Rigid2d& pose) + void ComputeConstraintsForScan(const Submap* matching_submap, + std::vector insertion_submaps, + const Submap* finished_submap, + const transform::Rigid2d& pose) REQUIRES(mutex_); // Computes constraints for a scan and submap pair. @@ -135,8 +135,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const mapping::SubmapId& submap_id) REQUIRES(mutex_); // Adds constraints for older scans whenever a new submap is finished. - void ComputeConstraintsForOldScans(const mapping::Submap* submap) - REQUIRES(mutex_); + void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_); // Registers the callback to run the optimization once all constraints have // been computed, that will also do all work that queue up in 'scan_queue_'. @@ -185,8 +184,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Submaps get assigned an ID and state as soon as they are seen, even // before they take part in the background computations. - std::map submap_ids_ - GUARDED_BY(mutex_); + std::map submap_ids_ GUARDED_BY(mutex_); mapping::NestedVectorsById submap_data_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 8e40572..3d3c95d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -38,7 +38,7 @@ namespace cartographer { namespace mapping_2d { namespace sparse_pose_graph { -transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) { +transform::Rigid2d ComputeSubmapPose(const Submap& submap) { return transform::Project2D(submap.local_pose()); } @@ -60,7 +60,7 @@ ConstraintBuilder::~ConstraintBuilder() { } void ConstraintBuilder::MaybeAddConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap* const submap, + const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud, const transform::Rigid2d& initial_relative_pose) { if (initial_relative_pose.translation().norm() > @@ -74,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) { + submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ @@ -85,7 +85,7 @@ void ConstraintBuilder::MaybeAddConstraint( } void ConstraintBuilder::MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap* const submap, + const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud, mapping::TrajectoryConnectivity* const trajectory_connectivity) { common::MutexLocker locker(&mutex_); @@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) { + submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ trajectory_connectivity, point_cloud, @@ -159,7 +159,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { } void ConstraintBuilder::ComputeConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap* const submap, + const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* const point_cloud, diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index d46bfdf..6cece92 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -35,7 +35,7 @@ #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" - +#include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" @@ -47,7 +47,7 @@ namespace sparse_pose_graph { // Returns (map <- submap) where 'submap' is a coordinate system at the origin // of the Submap. -transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap); +transform::Rigid2d ComputeSubmapPose(const Submap& submap); // Asynchronously computes constraints. // @@ -78,8 +78,7 @@ class ConstraintBuilder { // The pointees of 'submap' and 'point_cloud' must stay valid until all // computations are finished. void MaybeAddConstraint(const mapping::SubmapId& submap_id, - const mapping::Submap* submap, - const mapping::NodeId& node_id, + const Submap* submap, const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud, const transform::Rigid2d& initial_relative_pose); @@ -92,7 +91,7 @@ class ConstraintBuilder { // The pointees of 'submap' and 'point_cloud' must stay valid until all // computations are finished. void MaybeAddGlobalConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap* submap, + const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud, mapping::TrajectoryConnectivity* trajectory_connectivity); @@ -138,7 +137,7 @@ class ConstraintBuilder { // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( - const mapping::SubmapId& submap_id, const mapping::Submap* submap, + const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* point_cloud, diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 387dc4a..8563176 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -149,9 +149,9 @@ class SparsePoseGraphTest : public ::testing::Test { const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); - const mapping::Submap* const matching_submap = + const Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); - std::vector insertion_submaps; + std::vector insertion_submaps; for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } @@ -161,9 +161,14 @@ class SparsePoseGraphTest : public ::testing::Test { constexpr int kTrajectoryId = 0; submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); - sparse_pose_graph_->AddScan( - common::FromUniversal(0), transform::Rigid3d::Identity(), range_data, - pose_estimate, kTrajectoryId, matching_submap, insertion_submaps); + + const Submap* const finished_submap = insertion_submaps.front()->finished() + ? insertion_submaps.front() + : nullptr; + sparse_pose_graph_->AddScan(common::FromUniversal(0), + transform::Rigid3d::Identity(), range_data, + pose_estimate, kTrajectoryId, matching_submap, + insertion_submaps, finished_submap); } void MoveRelative(const transform::Rigid2d& movement) { diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index 8a49b2d..577769c 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -162,7 +162,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options) void Submaps::InsertRangeData(const sensor::RangeData& range_data) { for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); - CHECK(submap->finished_probability_grid_ == nullptr); + CHECK(!submap->finished_); range_data_inserter_.Insert(range_data, &submap->probability_grid_); ++submap->num_range_data_; } @@ -184,10 +184,10 @@ void Submaps::FinishSubmap(int index) { // Crop the finished Submap before inserting a new Submap to reduce peak // memory usage a bit. Submap* submap = submaps_[index].get(); - CHECK(submap->finished_probability_grid_ == nullptr); + CHECK(!submap->finished_); submap->probability_grid_ = ComputeCroppedProbabilityGrid(submap->probability_grid_); - submap->finished_probability_grid_ = &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", diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index 00ed6fc..23205ad 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -46,6 +46,7 @@ class Submap : public mapping::Submap { Submap(const MapLimits& limits, const Eigen::Vector2f& origin); const ProbabilityGrid& probability_grid() const { return probability_grid_; } + const bool finished() const { return finished_; } void ToResponseProto( const transform::Rigid3d& global_submap_pose, @@ -56,6 +57,7 @@ class Submap : public mapping::Submap { friend class Submaps; ProbabilityGrid probability_grid_; + bool finished_ = false; }; // A container of Submaps. diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index eaee4fc..d548945 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -55,15 +55,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData( const sensor::PointCloud& ranges) { auto insertion_result = local_trajectory_builder_->AddRangefinderData(time, origin, ranges); - if (insertion_result == nullptr) { return; } + const Submap* const finished_submap = + insertion_result->insertion_submaps.front()->finished() + ? insertion_result->insertion_submaps.front() + : nullptr; sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->range_data_in_tracking, insertion_result->pose_observation, trajectory_id_, - insertion_result->matching_submap, insertion_result->insertion_submaps); + insertion_result->matching_submap, insertion_result->insertion_submaps, + finished_submap); } void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 35f02c0..1865ec7 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -90,7 +90,8 @@ void SparsePoseGraph::AddScan( common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, const int trajectory_id, const Submap* const matching_submap, - const std::vector& insertion_submaps) { + const std::vector& insertion_submaps, + const Submap* const finished_submap) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * pose); @@ -113,9 +114,6 @@ void SparsePoseGraph::AddScan( submap_ids_.emplace(insertion_submaps.back(), submap_id); submap_data_.at(submap_id).submap = insertion_submaps.back(); } - const Submap* const finished_submap = insertion_submaps.front()->finished() - ? insertion_submaps.front() - : nullptr; // Make sure we have a sampler for this trajectory. if (!global_localization_samplers_[trajectory_id]) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index e1c8c28..6330ae9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -63,13 +63,15 @@ 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'. + // 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a freshly + // finished submap. It is also contained in 'insertion_submaps' for the last + // time. void AddScan(common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, int trajectory_id, const Submap* matching_submap, - const std::vector& insertion_submaps) - EXCLUDES(mutex_); + const std::vector& insertion_submaps, + const Submap* finished_submap) EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. void AddImuData(int trajectory_id, common::Time time,