From 213882a9b3d8aee10b7ea059e732f1632d3684de Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 13 Aug 2018 10:38:33 +0200 Subject: [PATCH] Rename submap finished flags (#1384) --- cartographer/cloud/internal/map_builder_server.cc | 2 +- .../cloud/internal/sensor/serialization.cc | 2 +- .../io/internal/mapping_state_serialization.cc | 2 +- cartographer/mapping/2d/submap_2d.cc | 14 +++++++------- cartographer/mapping/2d/submap_2d_test.cc | 2 +- cartographer/mapping/3d/submap_3d.cc | 12 ++++++------ cartographer/mapping/3d/submap_3d_test.cc | 2 +- .../internal/2d/overlapping_submaps_trimmer_2d.cc | 2 +- cartographer/mapping/internal/2d/pose_graph_2d.cc | 10 ++++++---- cartographer/mapping/internal/3d/pose_graph_3d.cc | 10 ++++++---- cartographer/mapping/internal/submap_controller.h | 2 +- cartographer/mapping/pose_graph.cc | 2 +- cartographer/mapping/pose_graph_data.h | 13 +++++++------ cartographer/mapping/submaps.h | 15 ++++++++------- 14 files changed, 48 insertions(+), 42 deletions(-) diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index a5df785..bb96864 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/cartographer/cloud/internal/map_builder_server.cc @@ -192,7 +192,7 @@ void MapBuilderServer::OnLocalSlamResult( time, starting_submap_index_, *insertion_result, sensor_data.get()); // TODO(cschuet): Make this more robust. - if (insertion_result->insertion_submaps.front()->finished()) { + if (insertion_result->insertion_submaps.front()->insertion_finished()) { ++starting_submap_index_; } grpc_server_->GetUnsynchronizedContext() diff --git a/cartographer/cloud/internal/sensor/serialization.cc b/cartographer/cloud/internal/sensor/serialization.cc index 39a7acc..429ba68 100644 --- a/cartographer/cloud/internal/sensor/serialization.cc +++ b/cartographer/cloud/internal/sensor/serialization.cc @@ -92,7 +92,7 @@ void CreateSensorDataForLocalSlamResult( for (const auto& insertion_submap : insertion_result.insertion_submaps) { // We only send the probability grid up if the submap is finished. auto* submap = proto->mutable_local_slam_result_data()->add_submaps(); - *submap = insertion_submap->ToProto(insertion_submap->finished()); + *submap = insertion_submap->ToProto(insertion_submap->insertion_finished()); submap->mutable_submap_id()->set_trajectory_id(trajectory_id); submap->mutable_submap_id()->set_submap_index(starting_submap_index); ++starting_submap_index; diff --git a/cartographer/io/internal/mapping_state_serialization.cc b/cartographer/io/internal/mapping_state_serialization.cc index 7f89713..a34c867 100644 --- a/cartographer/io/internal/mapping_state_serialization.cc +++ b/cartographer/io/internal/mapping_state_serialization.cc @@ -84,7 +84,7 @@ void SerializeSubmaps( // Next serialize all submaps. for (const auto& submap_id_data : submap_data) { if (!include_unfinished_submaps && - !submap_id_data.data.submap->finished()) { + !submap_id_data.data.submap->insertion_finished()) { continue; } SerializedData proto; diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index 82591cf..feb30fc 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -90,7 +90,7 @@ Submap2D::Submap2D(const proto::Submap2D& proto, } } set_num_range_data(proto.num_range_data()); - set_finished(proto.finished()); + set_insertion_finished(proto.finished()); } proto::Submap Submap2D::ToProto(const bool include_grid_data) const { @@ -98,7 +98,7 @@ proto::Submap Submap2D::ToProto(const bool include_grid_data) const { auto* const submap_2d = proto.mutable_submap_2d(); *submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); submap_2d->set_num_range_data(num_range_data()); - submap_2d->set_finished(finished()); + submap_2d->set_finished(insertion_finished()); if (include_grid_data) { CHECK(grid_); *submap_2d->mutable_grid() = grid_->ToProto(); @@ -110,7 +110,7 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) { CHECK(proto.has_submap_2d()); const auto& submap_2d = proto.submap_2d(); set_num_range_data(submap_2d.num_range_data()); - set_finished(submap_2d.finished()); + set_insertion_finished(submap_2d.finished()); if (proto.submap_2d().has_grid()) { if (proto.submap_2d().grid().has_probability_grid_2d()) { grid_ = absl::make_unique(proto.submap_2d().grid(), @@ -138,16 +138,16 @@ void Submap2D::InsertRangeData( const sensor::RangeData& range_data, const RangeDataInserterInterface* range_data_inserter) { CHECK(grid_); - CHECK(!finished()); + CHECK(!insertion_finished()); range_data_inserter->Insert(range_data, grid_.get()); set_num_range_data(num_range_data() + 1); } void Submap2D::Finish() { CHECK(grid_); - CHECK(!finished()); + CHECK(!insertion_finished()); grid_ = grid_->ComputeCroppedGrid(); - set_finished(true); + set_insertion_finished(true); } ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options) @@ -225,7 +225,7 @@ void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) { if (submaps_.size() >= 2) { // This will crop the finished Submap before inserting a new Submap to // reduce peak memory usage a bit. - CHECK(submaps_.front()->finished()); + CHECK(submaps_.front()->insertion_finished()); submaps_.erase(submaps_.begin()); } submaps_.push_back(absl::make_unique( diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 6f773dc..1919d3a 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -113,7 +113,7 @@ TEST(Submap2DTest, ToFromProto) { EXPECT_TRUE(expected.local_pose().rotation().isApprox( actual.local_pose().rotation(), 1e-6)); EXPECT_EQ(expected.num_range_data(), actual.num_range_data()); - EXPECT_EQ(expected.finished(), actual.finished()); + EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished()); EXPECT_NEAR(expected.grid()->limits().resolution(), actual.grid()->limits().resolution(), 1e-6); EXPECT_TRUE(expected.grid()->limits().max().isApprox( diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 26878cc..7313bd5 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -214,7 +214,7 @@ proto::Submap Submap3D::ToProto( auto* const submap_3d = proto.mutable_submap_3d(); *submap_3d->mutable_local_pose() = transform::ToProto(local_pose()); submap_3d->set_num_range_data(num_range_data()); - submap_3d->set_finished(finished()); + submap_3d->set_finished(insertion_finished()); if (include_probability_grid_data) { *submap_3d->mutable_high_resolution_hybrid_grid() = high_resolution_hybrid_grid().ToProto(); @@ -231,7 +231,7 @@ void Submap3D::UpdateFromProto(const proto::Submap& proto) { void Submap3D::UpdateFromProto(const proto::Submap3D& submap_3d) { set_num_range_data(submap_3d.num_range_data()); - set_finished(submap_3d.finished()); + set_insertion_finished(submap_3d.finished()); if (submap_3d.has_high_resolution_hybrid_grid()) { high_resolution_hybrid_grid_ = absl::make_unique(submap_3d.high_resolution_hybrid_grid()); @@ -256,7 +256,7 @@ void Submap3D::ToResponseProto( void Submap3D::InsertRangeData(const sensor::RangeData& range_data, const RangeDataInserter3D& range_data_inserter, const float high_resolution_max_range) { - CHECK(!finished()); + CHECK(!insertion_finished()); const sensor::RangeData transformed_range_data = sensor::TransformRangeData( range_data, local_pose().inverse().cast()); range_data_inserter.Insert( @@ -269,8 +269,8 @@ void Submap3D::InsertRangeData(const sensor::RangeData& range_data, } void Submap3D::Finish() { - CHECK(!finished()); - set_finished(true); + CHECK(!insertion_finished()); + set_insertion_finished(true); } ActiveSubmaps3D::ActiveSubmaps3D(const proto::SubmapsOptions3D& options) @@ -304,7 +304,7 @@ void ActiveSubmaps3D::AddSubmap(const transform::Rigid3d& local_submap_pose) { if (submaps_.size() >= 2) { // This will crop the finished Submap before inserting a new Submap to // reduce peak memory usage a bit. - CHECK(submaps_.front()->finished()); + CHECK(submaps_.front()->insertion_finished()); submaps_.erase(submaps_.begin()); } submaps_.emplace_back(new Submap3D(options_.high_resolution(), diff --git a/cartographer/mapping/3d/submap_3d_test.cc b/cartographer/mapping/3d/submap_3d_test.cc index 3868d95..fdc53e4 100644 --- a/cartographer/mapping/3d/submap_3d_test.cc +++ b/cartographer/mapping/3d/submap_3d_test.cc @@ -38,7 +38,7 @@ TEST(SubmapsTest, ToFromProto) { EXPECT_TRUE(expected.local_pose().rotation().isApprox( actual.local_pose().rotation(), 1e-6)); EXPECT_EQ(expected.num_range_data(), actual.num_range_data()); - EXPECT_EQ(expected.finished(), actual.finished()); + EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished()); EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6); EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6); } diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index 863e428..3c5640e 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -60,7 +60,7 @@ std::set AddSubmapsToSubmapCoverageGrid2D( for (const auto& submap : submap_data) { auto freshness = submap_freshness.find(submap.id); if (freshness == submap_freshness.end()) continue; - if (!submap.data.submap->finished()) continue; + if (!submap.data.submap->insertion_finished()) continue; all_submap_ids.insert(submap.id); const Grid2D& grid = *std::static_pointer_cast(submap.data.submap)->grid(); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 644c210..814103a 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -146,7 +146,8 @@ NodeId PoseGraph2D::AddNode( insertion_submaps, optimized_pose); // We have to check this here, because it might have changed by the time we // execute the lambda. - const bool newly_finished_submap = insertion_submaps.front()->finished(); + const bool newly_finished_submap = + insertion_submaps.front()->insertion_finished(); AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); @@ -242,7 +243,7 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id, { absl::MutexLock locker(&mutex_); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); - if (!data_.submap_data.at(submap_id).submap->finished()) { + if (!data_.submap_data.at(submap_id).submap->insertion_finished()) { // Uplink server only receives grids when they are finished, so skip // constraint search before that. return; @@ -316,7 +317,8 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode( const SubmapId submap_id = submap_ids[i]; // Even if this was the last node added to 'submap_id', the submap will // only be marked as finished in 'data_.submap_data' further below. - CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive); + CHECK(data_.submap_data.at(submap_id).state == + SubmapState::kNoConstraintSearch); data_.submap_data.at(submap_id).node_ids.emplace(node_id); const transform::Rigid2d constraint_transform = constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * @@ -344,7 +346,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode( const SubmapId newly_finished_submap_id = submap_ids.front(); InternalSubmapData& finished_submap_data = data_.submap_data.at(newly_finished_submap_id); - CHECK(finished_submap_data.state == SubmapState::kActive); + CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch); finished_submap_data.state = SubmapState::kFinished; newly_finished_submap_node_ids = finished_submap_data.node_ids; } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 151afe9..2e3c2e4 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -143,7 +143,8 @@ NodeId PoseGraph3D::AddNode( insertion_submaps, optimized_pose); // We have to check this here, because it might have changed by the time we // execute the lambda. - const bool newly_finished_submap = insertion_submaps.front()->finished(); + const bool newly_finished_submap = + insertion_submaps.front()->insertion_finished(); AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); @@ -256,7 +257,7 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id, { absl::MutexLock locker(&mutex_); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); - if (!data_.submap_data.at(submap_id).submap->finished()) { + if (!data_.submap_data.at(submap_id).submap->insertion_finished()) { // Uplink server only receives grids when they are finished, so skip // constraint search before that. return; @@ -334,7 +335,8 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode( const SubmapId submap_id = submap_ids[i]; // Even if this was the last node added to 'submap_id', the submap will // only be marked as finished in 'data_.submap_data' further below. - CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive); + CHECK(data_.submap_data.at(submap_id).state == + SubmapState::kNoConstraintSearch); data_.submap_data.at(submap_id).node_ids.emplace(node_id); const transform::Rigid3d constraint_transform = insertion_submaps[i]->local_pose().inverse() * local_pose; @@ -359,7 +361,7 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode( const SubmapId newly_finished_submap_id = submap_ids.front(); InternalSubmapData& finished_submap_data = data_.submap_data.at(newly_finished_submap_id); - CHECK(finished_submap_data.state == SubmapState::kActive); + CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch); finished_submap_data.state = SubmapState::kFinished; newly_finished_submap_node_ids = finished_submap_data.node_ids; } diff --git a/cartographer/mapping/internal/submap_controller.h b/cartographer/mapping/internal/submap_controller.h index 4efdc8e..88922cd 100644 --- a/cartographer/mapping/internal/submap_controller.h +++ b/cartographer/mapping/internal/submap_controller.h @@ -47,7 +47,7 @@ class SubmapController { // If the submap was just finished by the recent update, remove it from // the list of unfinished submaps. - if (submap_ptr->finished()) { + if (submap_ptr->insertion_finished()) { unfinished_submaps_.Trim(submap_id); } else { // If the submap is unfinished set the 'num_range_data' to 0 since we diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index ce770ef..05ecd00 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -134,7 +134,7 @@ proto::PoseGraph PoseGraph::ToProto(bool include_unfinished_submaps) const { proto::Trajectory* trajectory_proto = trajectory(submap_id_data.id.trajectory_id); if (!include_unfinished_submaps && - !submap_id_data.data.submap->finished()) { + !submap_id_data.data.submap->insertion_finished()) { // Collect IDs of all unfinished submaps and skip them. unfinished_submaps.insert(submap_id_data.id); continue; diff --git a/cartographer/mapping/pose_graph_data.h b/cartographer/mapping/pose_graph_data.h index 6d2ffbd..666f942 100644 --- a/cartographer/mapping/pose_graph_data.h +++ b/cartographer/mapping/pose_graph_data.h @@ -31,10 +31,11 @@ namespace cartographer { namespace mapping { -// The current state of the submap in the background threads. When this -// transitions to kFinished, all nodes are tried to match against this submap. -// Likewise, all new nodes are matched against submaps which are finished. -enum class SubmapState { kActive, kFinished }; +// The current state of the submap in the background threads. After this +// transitions to 'kFinished', all nodes are tried to match +// against this submap. Likewise, all new nodes are matched against submaps in +// that state. +enum class SubmapState { kNoConstraintSearch, kFinished }; struct InternalTrajectoryState { enum class DeletionState { @@ -50,11 +51,11 @@ struct InternalTrajectoryState { struct InternalSubmapData { std::shared_ptr submap; - SubmapState state = SubmapState::kActive; + SubmapState state = SubmapState::kNoConstraintSearch; // IDs of the nodes that were inserted into this map together with // constraints for them. They are not to be matched again when this submap - // becomes 'finished'. + // becomes 'kFinished'. std::set node_ids; }; diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index 76e6c9a..39ca93b 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -53,9 +53,9 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) { } // An individual submap, which has a 'local_pose' in the local map frame, keeps -// track of 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. +// track of how many range data were inserted into it, and sets +// 'insertion_finished' when the map no longer changes and is ready for loop +// closing. class Submap { public: Submap(const transform::Rigid3d& local_submap_pose) @@ -79,14 +79,15 @@ class Submap { num_range_data_ = num_range_data; } - // Whether the submap is finished or not. - bool finished() const { return finished_; } - void set_finished(bool finished) { finished_ = finished; } + bool insertion_finished() const { return insertion_finished_; } + void set_insertion_finished(bool insertion_finished) { + insertion_finished_ = insertion_finished; + } private: const transform::Rigid3d local_pose_; int num_range_data_ = 0; - bool finished_ = false; + bool insertion_finished_ = false; }; } // namespace mapping