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

Enable loading unfrozen state (#891) 

Enable loading unfrozen state
master
Juraj Oršulić 2018-02-27 20:38:57 +01:00 committed by Christoph Schütte
parent 29875117b3
commit 32b8bd3581
16 changed files with 128 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/io/in_memory_proto_stream.cc
View File

@ -30,8 +30,8 @@ bool ForwardingProtoStreamWriter::Close() { return writer_callback_(nullptr); }
bool InMemoryProtoStreamReader::ReadProto(google::protobuf::Message* proto) {
if (eof()) return false;
proto->CopyFrom(*map_chunks_.front());
map_chunks_.pop();
proto->CopyFrom(*state_chunks_.front());
state_chunks_.pop();
return true;
}

10
cartographer/io/in_memory_proto_stream.h
View File

@ -51,8 +51,8 @@ class InMemoryProtoStreamReader
: public cartographer::io::ProtoStreamReaderInterface {
public:
explicit InMemoryProtoStreamReader(
std::queue<std::unique_ptr<google::protobuf::Message>>&& map_chunks)
: map_chunks_(std::move(map_chunks)){};
std::queue<std::unique_ptr<google::protobuf::Message>>&& state_chunks)
: state_chunks_(std::move(state_chunks)){};
InMemoryProtoStreamReader() = default;
~InMemoryProtoStreamReader() = default;
@ -62,14 +62,14 @@ class InMemoryProtoStreamReader
template <typename MessageType>
void AddProto(const MessageType& proto) {
map_chunks_.push(common::make_unique<MessageType>(proto));
state_chunks_.push(common::make_unique<MessageType>(proto));
}
bool ReadProto(google::protobuf::Message* proto) override;
bool eof() const override { return map_chunks_.empty(); }
bool eof() const override { return state_chunks_.empty(); }
private:
std::queue<std::unique_ptr<google::protobuf::Message>> map_chunks_;
std::queue<std::unique_ptr<google::protobuf::Message>> state_chunks_;
};
} // namespace io

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

@ -439,7 +439,6 @@ void PoseGraph2D::AddSubmapFromProto(
submap_id,
pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d);
});
@ -457,7 +456,6 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse());

2
cartographer/mapping/3d/pose_graph_3d.cc
View File

@ -455,7 +455,6 @@ void PoseGraph3D::AddSubmapFromProto(
submap_id,
pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose});
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.InsertSubmap(submap_id, global_submap_pose);
});
@ -473,7 +472,6 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.InsertTrajectoryNode(
node_id, constant_data->time, constant_data->local_pose, global_pose);

71
cartographer/mapping/map_builder.cc
View File

@ -276,9 +276,11 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
writer->WriteProto(proto);
}
}
// TODO(pifon2a, ojura): serialize landmarks
}
void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
bool load_frozen_state) {
proto::PoseGraph pose_graph_proto;
CHECK(reader->ReadProto(&pose_graph_proto));
proto::AllTrajectoryBuilderOptions all_builder_options_proto;
@ -298,7 +300,17 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
.second)
<< "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
trajectory_proto.set_trajectory_id(new_trajectory_id);
pose_graph_->FreezeTrajectory(new_trajectory_id);
if (load_frozen_state) {
pose_graph_->FreezeTrajectory(new_trajectory_id);
}
}
// Apply the calculated remapping to constraints in the pose graph proto.
for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) {
constraint_proto.mutable_submap_id()->set_trajectory_id(
trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()));
constraint_proto.mutable_node_id()->set_trajectory_id(
trajectory_remapping.at(constraint_proto.node_id().trajectory_id()));
}
MapById<SubmapId, transform::Rigid3d> submap_poses;
@ -348,24 +360,49 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
trajectory_remapping.at(proto.trajectory_data().trajectory_id()));
pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data());
}
// TODO(ojura): Deserialize IMU, odometry and fixed frame pose data when
// loading unfrozen trajectories.
if (!load_frozen_state) {
if (proto.has_imu_data()) {
pose_graph_->AddImuData(
trajectory_remapping.at(proto.imu_data().trajectory_id()),
sensor::FromProto(proto.imu_data().imu_data()));
}
if (proto.has_odometry_data()) {
pose_graph_->AddOdometryData(
trajectory_remapping.at(proto.odometry_data().trajectory_id()),
sensor::FromProto(proto.odometry_data().odometry_data()));
}
if (proto.has_fixed_frame_pose_data()) {
pose_graph_->AddFixedFramePoseData(
trajectory_remapping.at(
proto.fixed_frame_pose_data().trajectory_id()),
sensor::FromProto(
proto.fixed_frame_pose_data().fixed_frame_pose_data()));
}
// TODO(pifon2a, ojura): deserialize landmarks
}
}
// Add information about which nodes belong to which submap.
for (const proto::PoseGraph::Constraint& constraint_proto :
pose_graph_proto.constraint()) {
if (constraint_proto.tag() !=
mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) {
continue;
if (load_frozen_state) {
// Add information about which nodes belong to which submap.
// Required for 3D pure localization.
for (const proto::PoseGraph::Constraint& constraint_proto :
pose_graph_proto.constraint()) {
if (constraint_proto.tag() !=
mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) {
continue;
}
pose_graph_->AddNodeToSubmap(
NodeId{constraint_proto.node_id().trajectory_id(),
constraint_proto.node_id().node_index()},
SubmapId{constraint_proto.submap_id().trajectory_id(),
constraint_proto.submap_id().submap_index()});
}
const NodeId node_id{
trajectory_remapping.at(constraint_proto.node_id().trajectory_id()),
constraint_proto.node_id().node_index()};
const SubmapId submap_id{
trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()),
constraint_proto.submap_id().submap_index()};
pose_graph_->AddNodeToSubmap(node_id, submap_id);
} else {
// When loading unfrozen trajectories, 'AddSerializedConstraints' will
// take care of adding information about which nodes belong to which
// submap.
pose_graph_->AddSerializedConstraints(
FromProto(pose_graph_proto.constraint()));
}
CHECK(reader->eof());
}

3
cartographer/mapping/map_builder.h
View File

@ -63,7 +63,8 @@ class MapBuilder : public MapBuilderInterface {
void SerializeState(io::ProtoStreamWriterInterface* writer) override;
void LoadMap(io::ProtoStreamReaderInterface* reader) override;
void LoadState(io::ProtoStreamReaderInterface* reader,
bool load_frozen_state) override;
int num_trajectory_builders() const override;

5
cartographer/mapping/map_builder_interface.h
View File

@ -80,8 +80,9 @@ class MapBuilderInterface {
// Serializes the current state to a proto stream.
virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0;
// Loads submaps from a proto stream into a new frozen trajectory.
virtual void LoadMap(io::ProtoStreamReaderInterface* reader) = 0;
// Loads the SLAM state from a proto stream.
virtual void LoadState(io::ProtoStreamReaderInterface* reader,
bool load_frozen_state) = 0;
virtual int num_trajectory_builders() const = 0;

19
cartographer_grpc/handlers/load_map_handler.cc → cartographer_grpc/handlers/load_state_handler.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer_grpc/handlers/load_map_handler.h"
#include "cartographer_grpc/handlers/load_state_handler.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/io/in_memory_proto_stream.h"
@ -26,24 +26,25 @@
namespace cartographer_grpc {
namespace handlers {
void LoadMapHandler::OnRequest(const proto::LoadMapRequest& request) {
switch (request.map_chunk_case()) {
case proto::LoadMapRequest::kPoseGraph:
void LoadStateHandler::OnRequest(const proto::LoadStateRequest& request) {
switch (request.state_chunk_case()) {
case proto::LoadStateRequest::kPoseGraph:
reader_.AddProto(request.pose_graph());
break;
case proto::LoadMapRequest::kAllTrajectoryBuilderOptions:
case proto::LoadStateRequest::kAllTrajectoryBuilderOptions:
reader_.AddProto(request.all_trajectory_builder_options());
break;
case proto::LoadMapRequest::kSerializedData:
case proto::LoadStateRequest::kSerializedData:
reader_.AddProto(request.serialized_data());
break;
default:
LOG(FATAL) << "Unhandled proto::LoadMapRequest case.";
LOG(FATAL) << "Unhandled proto::LoadStateRequest case.";
}
}
void LoadMapHandler::OnReadsDone() {
GetContext<MapBuilderContextInterface>()->map_builder().LoadMap(&reader_);
void LoadStateHandler::OnReadsDone() {
GetContext<MapBuilderContextInterface>()->map_builder().LoadState(&reader_,
true);
Send(cartographer::common::make_unique<google::protobuf::Empty>());
}

14
cartographer_grpc/handlers/load_map_handler.h → cartographer_grpc/handlers/load_state_handler.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H
#define CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H
#ifndef CARTOGRAPHER_GRPC_HANDLERS_LOAD_STATE_HANDLER_H
#define CARTOGRAPHER_GRPC_HANDLERS_LOAD_STATE_HANDLER_H
#include "cartographer/io/in_memory_proto_stream.h"
#include "cartographer_grpc/framework/rpc_handler.h"
@ -25,14 +25,14 @@
namespace cartographer_grpc {
namespace handlers {
class LoadMapHandler
: public framework::RpcHandler<framework::Stream<proto::LoadMapRequest>,
class LoadStateHandler
: public framework::RpcHandler<framework::Stream<proto::LoadStateRequest>,
google::protobuf::Empty> {
public:
std::string method_name() const override {
return "/cartographer_grpc.proto.MapBuilderService/LoadMap";
return "/cartographer_grpc.proto.MapBuilderService/LoadState";
}
void OnRequest(const proto::LoadMapRequest& request) override;
void OnRequest(const proto::LoadStateRequest& request) override;
void OnReadsDone() override;
private:
@ -42,4 +42,4 @@ class LoadMapHandler
} // namespace handlers
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H
#endif // CARTOGRAPHER_GRPC_HANDLERS_LOAD_STATE_HANDLER_H

6
cartographer_grpc/handlers/write_map_handler.cc → cartographer_grpc/handlers/write_state_handler.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer_grpc/handlers/write_map_handler.h"
#include "cartographer_grpc/handlers/write_state_handler.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/io/in_memory_proto_stream.h"
@ -26,7 +26,7 @@
namespace cartographer_grpc {
namespace handlers {
void WriteMapHandler::OnRequest(const google::protobuf::Empty& request) {
void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) {
auto writer = GetWriter();
cartographer::io::ForwardingProtoStreamWriter proto_stream_writer(
[writer](const google::protobuf::Message* proto) {
@ -36,7 +36,7 @@ void WriteMapHandler::OnRequest(const google::protobuf::Empty& request) {
}
auto response =
cartographer::common::make_unique<proto::WriteMapResponse>();
cartographer::common::make_unique<proto::WriteStateResponse>();
if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") {
response->mutable_pose_graph()->CopyFrom(*proto);
} else if (proto->GetTypeName() ==

14
cartographer_grpc/handlers/write_map_handler.h → cartographer_grpc/handlers/write_state_handler.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H
#define CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H
#ifndef CARTOGRAPHER_GRPC_HANDLERS_WRITE_STATE_HANDLER_H
#define CARTOGRAPHER_GRPC_HANDLERS_WRITE_STATE_HANDLER_H
#include "cartographer_grpc/framework/rpc_handler.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
@ -24,12 +24,12 @@
namespace cartographer_grpc {
namespace handlers {
class WriteMapHandler
: public framework::RpcHandler<google::protobuf::Empty,
framework::Stream<proto::WriteMapResponse>> {
class WriteStateHandler : public framework::RpcHandler<
google::protobuf::Empty,
framework::Stream<proto::WriteStateResponse>> {
public:
std::string method_name() const override {
return "/cartographer_grpc.proto.MapBuilderService/WriteMap";
return "/cartographer_grpc.proto.MapBuilderService/WriteState";
}
void OnRequest(const google::protobuf::Empty& request) override;
};
@ -37,4 +37,4 @@ class WriteMapHandler
} // namespace handlers
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H
#endif // CARTOGRAPHER_GRPC_HANDLERS_WRITE_STATE_HANDLER_H

8
cartographer_grpc/map_builder_server.cc
View File

@ -30,10 +30,10 @@
#include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h"
#include "cartographer_grpc/handlers/get_submap_handler.h"
#include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h"
#include "cartographer_grpc/handlers/load_map_handler.h"
#include "cartographer_grpc/handlers/load_state_handler.h"
#include "cartographer_grpc/handlers/receive_local_slam_results_handler.h"
#include "cartographer_grpc/handlers/run_final_optimization_handler.h"
#include "cartographer_grpc/handlers/write_map_handler.h"
#include "cartographer_grpc/handlers/write_state_handler.h"
#include "cartographer_grpc/sensor/serialization.h"
#include "glog/logging.h"
@ -75,9 +75,9 @@ MapBuilderServer::MapBuilderServer(
server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>();
server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();
server_builder.RegisterHandler<handlers::GetConstraintsHandler>();
server_builder.RegisterHandler<handlers::LoadMapHandler>();
server_builder.RegisterHandler<handlers::LoadStateHandler>();
server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
server_builder.RegisterHandler<handlers::WriteMapHandler>();
server_builder.RegisterHandler<handlers::WriteStateHandler>();
grpc_server_ = server_builder.Build();
grpc_server_->SetExecutionContext(
cartographer::common::make_unique<MapBuilderContext>(this));

32
cartographer_grpc/mapping/map_builder_stub.cc
View File

@ -18,8 +18,8 @@
#include "cartographer_grpc/handlers/add_trajectory_handler.h"
#include "cartographer_grpc/handlers/finish_trajectory_handler.h"
#include "cartographer_grpc/handlers/get_submap_handler.h"
#include "cartographer_grpc/handlers/load_map_handler.h"
#include "cartographer_grpc/handlers/write_map_handler.h"
#include "cartographer_grpc/handlers/load_state_handler.h"
#include "cartographer_grpc/handlers/write_state_handler.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h"
#include "glog/logging.h"
@ -93,19 +93,19 @@ std::string MapBuilderStub::SubmapToProto(
void MapBuilderStub::SerializeState(
cartographer::io::ProtoStreamWriterInterface* writer) {
google::protobuf::Empty request;
framework::Client<handlers::WriteMapHandler> client(client_channel_);
framework::Client<handlers::WriteStateHandler> client(client_channel_);
CHECK(client.Write(request));
proto::WriteMapResponse response;
proto::WriteStateResponse response;
while (client.Read(&response)) {
// writer->WriteProto(response);
switch (response.map_chunk_case()) {
case proto::WriteMapResponse::kPoseGraph:
switch (response.state_chunk_case()) {
case proto::WriteStateResponse::kPoseGraph:
writer->WriteProto(response.pose_graph());
break;
case proto::WriteMapResponse::kAllTrajectoryBuilderOptions:
case proto::WriteStateResponse::kAllTrajectoryBuilderOptions:
writer->WriteProto(response.all_trajectory_builder_options());
break;
case proto::WriteMapResponse::kSerializedData:
case proto::WriteStateResponse::kSerializedData:
writer->WriteProto(response.serialized_data());
break;
default:
@ -115,23 +115,27 @@ void MapBuilderStub::SerializeState(
CHECK(writer->Close());
}
void MapBuilderStub::LoadMap(
cartographer::io::ProtoStreamReaderInterface* reader) {
framework::Client<handlers::LoadMapHandler> client(client_channel_);
void MapBuilderStub::LoadState(
cartographer::io::ProtoStreamReaderInterface* reader,
const bool load_frozen_state) {
if (!load_frozen_state) {
LOG(FATAL) << "Not implemented";
}
framework::Client<handlers::LoadStateHandler> client(client_channel_);
// Request with a PoseGraph proto is sent first.
{
proto::LoadMapRequest request;
proto::LoadStateRequest request;
CHECK(reader->ReadProto(request.mutable_pose_graph()));
CHECK(client.Write(request));
}
// Request with an AllTrajectoryBuilderOptions should be second.
{
proto::LoadMapRequest request;
proto::LoadStateRequest request;
CHECK(reader->ReadProto(request.mutable_all_trajectory_builder_options()));
CHECK(client.Write(request));
}
// Multiple requests with SerializedData are sent after.
proto::LoadMapRequest request;
proto::LoadStateRequest request;
while (reader->ReadProto(request.mutable_serialized_data())) {
CHECK(client.Write(request));
}

3
cartographer_grpc/mapping/map_builder_stub.h
View File

@ -50,7 +50,8 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface {
cartographer::mapping::proto::SubmapQuery::Response* response) override;
void SerializeState(
cartographer::io::ProtoStreamWriterInterface* writer) override;
void LoadMap(cartographer::io::ProtoStreamReaderInterface* reader) override;
void LoadState(cartographer::io::ProtoStreamReaderInterface* reader,
bool load_frozen_state) override;
int num_trajectory_builders() const override;
cartographer::mapping::PoseGraphInterface* pose_graph() override;
const std::vector<

17
cartographer_grpc/proto/map_builder_service.proto
View File

@ -102,8 +102,8 @@ message GetSubmapRequest {
cartographer.mapping.proto.SubmapId submap_id = 1;
}
message LoadMapRequest {
oneof map_chunk {
message LoadStateRequest {
oneof state_chunk {
cartographer.mapping.proto.PoseGraph pose_graph = 1;
cartographer.mapping.proto.AllTrajectoryBuilderOptions
all_trajectory_builder_options = 2;
@ -162,8 +162,8 @@ message AddLocalSlamResultDataRequest {
cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2;
}
message WriteMapResponse {
oneof map_chunk {
message WriteStateResponse {
oneof state_chunk {
cartographer.mapping.proto.PoseGraph pose_graph = 1;
cartographer.mapping.proto.AllTrajectoryBuilderOptions
all_trajectory_builder_options = 2;
@ -234,9 +234,10 @@ service MapBuilderService {
rpc RunFinalOptimization(google.protobuf.Empty)
returns (google.protobuf.Empty);
// Adds map data in the order defined by ProtoStreamReader.
rpc LoadMap(stream LoadMapRequest) returns (google.protobuf.Empty);
// Adds serialized SLAM state data in the order defined by ProtoStreamReader.
rpc LoadState(stream LoadStateRequest) returns (google.protobuf.Empty);
// Receives map data in the order defined by ProtoStreamWriter.
rpc WriteMap(google.protobuf.Empty) returns (stream WriteMapResponse);
// Receives serialized SLAM state data in the order defined by
// ProtoStreamWriter.
rpc WriteState(google.protobuf.Empty) returns (stream WriteStateResponse);
}

3
cartographer_grpc/testing/mock_map_builder.h
View File

@ -51,7 +51,8 @@ class MockMapBuilder : public cartographer::mapping::MapBuilderInterface {
cartographer::mapping::proto::SubmapQuery::Response *));
MOCK_METHOD1(SerializeState,
void(cartographer::io::ProtoStreamWriterInterface *));
MOCK_METHOD1(LoadMap, void(cartographer::io::ProtoStreamReaderInterface *));
MOCK_METHOD2(LoadState,
void(cartographer::io::ProtoStreamReaderInterface *, bool));
MOCK_CONST_METHOD0(num_trajectory_builders, int());
MOCK_METHOD0(pose_graph, cartographer::mapping::PoseGraphInterface *());
MOCK_CONST_METHOD0(