12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix writing state of trajectories with unfinished submaps. (#1286)

master
Christoph Schütte 2018-07-17 08:39:50 +02:00 committed by GitHub
parent 482f912d95
commit 0ee06ba561
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 23 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/io/internal/mapping_state_serialization.cc
View File

@ -88,8 +88,10 @@ void SerializeSubmaps(
submap_id_data.id.trajectory_id);
submap_proto->mutable_submap_id()->set_submap_index(
submap_id_data.id.submap_index);
submap_id_data.data.submap->ToProto(submap_proto,
/*include_probability_grid_data=*/true);
submap_id_data.data.submap->ToProto(
submap_proto,
/*include_probability_grid_data=*/submap_id_data.data.submap
->finished());
writer->WriteProto(proto);
}
}

13
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -560,11 +560,14 @@ void PoseGraph2D::AddSubmapFromProto(
// Immediately show the submap at the 'global_submap_pose'.
data_.global_submap_poses_2d.Insert(
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
return WorkItem::Result::kDoNotRunOptimization;
});
bool finished = submap.submap_2d().finished();
AddWorkItem(
[this, submap_id, global_submap_pose_2d, finished]() REQUIRES(mutex_) {
data_.submap_data.at(submap_id).state =
finished ? SubmapState::kFinished : SubmapState::kActive;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
return WorkItem::Result::kDoNotRunOptimization;
});
}
void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,

15
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -650,12 +650,15 @@ void PoseGraph3D::AddSubmapFromProto(
data_.global_submap_poses_3d.Insert(
submap_id, optimization::SubmapSpec3D{global_submap_pose});
}
AddWorkItem([this, submap_id, global_submap_pose]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
return WorkItem::Result::kDoNotRunOptimization;
});
bool finished = submap.submap_3d().finished();
AddWorkItem(
[this, submap_id, global_submap_pose, finished]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
data_.submap_data.at(submap_id).state =
finished ? SubmapState::kFinished : SubmapState::kActive;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
return WorkItem::Result::kDoNotRunOptimization;
});
}
void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,

1
cartographer/mapping/internal/testing/test_helpers.cc
View File

@ -85,6 +85,7 @@ proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index) {
proto.mutable_submap_3d()->set_num_range_data(1);
*proto.mutable_submap_3d()->mutable_local_pose() =
transform::ToProto(transform::Rigid3d::Identity());
proto.mutable_submap_3d()->set_finished(true);
return proto;
}

2
cartographer/mapping/map_builder_test.cc
View File

@ -417,7 +417,7 @@ TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) {
++num_cross_trajectory_constraints;
}
}
EXPECT_GT(num_cross_trajectory_constraints, 3);
EXPECT_EQ(num_cross_trajectory_constraints, 3);
// TODO(gaschler): Subscribe global slam callback, verify that all nodes are
// optimized.
EXPECT_THAT(constraints, ::testing::Contains(::testing::Field(