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

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

Reason: Break rviz visualization for submaps loaded from pbstreams.
master
Christoph Schütte 2018-07-27 17:20:42 +02:00 committed by Wally B. Feed
parent 3af5086dc7
commit dafb4149ed
6 changed files with 14 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cartographer/cloud/internal/client_server_test.cc
View File

@ -452,26 +452,6 @@ TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) {
}
WaitForLocalSlamResults(measurements.size());
WaitForLocalSlamResultUploads(number_of_insertion_results_);
std::queue<std::unique_ptr<google::protobuf::Message>> chunks;
io::ForwardingProtoStreamWriter writer(
[&chunks](const google::protobuf::Message* proto) -> bool {
if (!proto) {
return true;
}
std::unique_ptr<google::protobuf::Message> p(proto->New());
p->CopyFrom(*proto);
chunks.push(std::move(p));
return true;
});
stub_->SerializeState(&writer);
CHECK(writer.Close());
// Ensure it can be read.
io::InMemoryProtoStreamReader reader(std::move(chunks));
io::ProtoStreamDeserializer deserializer(&reader);
EXPECT_EQ(deserializer.pose_graph().trajectory_size(), 1);
stub_for_uploading_server_->FinishTrajectory(trajectory_id);
EXPECT_EQ(local_slam_result_poses_.size(), measurements.size());
EXPECT_NEAR(kTravelDistance,

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

@ -85,8 +85,7 @@ void SerializeSubmaps(
SerializedData proto;
auto* const submap_proto = proto.mutable_submap();
*submap_proto = submap_id_data.data.submap->ToProto(
/*include_probability_grid_data=*/submap_id_data.data.submap
->finished());
/*include_probability_grid_data=*/true);
submap_proto->mutable_submap_id()->set_trajectory_id(
submap_id_data.id.trajectory_id);
submap_proto->mutable_submap_id()->set_submap_index(

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

@ -636,12 +636,9 @@ void PoseGraph2D::AddSubmapFromProto(
data_.global_submap_poses_2d.Insert(
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
}
const bool finished = submap.submap_2d().finished();
AddWorkItem(
[this, submap_id, global_submap_pose_2d, finished]() EXCLUDES(mutex_) {
AddWorkItem([this, submap_id, global_submap_pose_2d]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
data_.submap_data.at(submap_id).state =
finished ? SubmapState::kFinished : SubmapState::kActive;
data_.submap_data.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
return WorkItem::Result::kDoNotRunOptimization;
});

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

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

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

@ -85,7 +85,6 @@ 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_EQ(num_cross_trajectory_constraints, 3);
EXPECT_GT(num_cross_trajectory_constraints, 3);
// TODO(gaschler): Subscribe global slam callback, verify that all nodes are
// optimized.
EXPECT_THAT(constraints, ::testing::Contains(::testing::Field(