Rename submap finished flags (#1384)

master
gaschler 2018-08-13 10:38:33 +02:00 committed by Alexander Belyaev
parent 811f2e8e8f
commit 213882a9b3
14 changed files with 48 additions and 42 deletions

View File

@ -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<MapBuilderContextInterface>()

View File

@ -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;

View File

@ -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;

View File

@ -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<ProbabilityGrid>(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<Submap2D>(

View File

@ -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(

View File

@ -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<HybridGrid>(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<float>());
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(),

View File

@ -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);
}

View File

@ -60,7 +60,7 @@ std::set<SubmapId> 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<const Submap2D>(submap.data.submap)->grid();

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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

View File

@ -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;

View File

@ -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<const Submap> 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<NodeId> node_ids;
};

View File

@ -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