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

Serialize landmark observations. (#1006)

master
Alexander Belyaev 2018-03-19 15:12:47 +01:00 committed by GitHub
parent 7a7908ebb9
commit 4cc758e830
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 43 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -635,6 +635,12 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() {
return optimization_problem_->odometry_data();
}
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
PoseGraph2D::GetLandmarkNodes() {
common::MutexLocker locker(&mutex_);
return landmark_nodes_;
}
std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph2D::GetTrajectoryData() {
return {}; // Not implemented yet in 2D.

2
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -126,6 +126,8 @@ class PoseGraph2D : public PoseGraph {
EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
EXCLUDES(mutex_);
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() override EXCLUDES(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,

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

@ -684,6 +684,12 @@ PoseGraph3D::GetFixedFramePoseData() {
return optimization_problem_->fixed_frame_pose_data();
}
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
PoseGraph3D::GetLandmarkNodes() {
common::MutexLocker locker(&mutex_);
return landmark_nodes_;
}
std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph3D::GetTrajectoryData() {
common::MutexLocker locker(&mutex_);

2
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -125,6 +125,8 @@ class PoseGraph3D : public PoseGraph {
EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
EXCLUDES(mutex_);
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() override EXCLUDES(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() override;
std::vector<Constraint> constraints() override EXCLUDES(mutex_);

24
cartographer/mapping/map_builder.cc
View File

@ -286,7 +286,29 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
writer->WriteProto(proto);
}
}
// TODO(pifon2a, ojura): serialize landmarks
// Next we serialize all landmark data.
{
const std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
all_landmark_nodes = pose_graph_->GetLandmarkNodes();
for (const auto& node : all_landmark_nodes) {
for (const auto& observation : node.second.landmark_observations) {
proto::SerializedData proto;
auto* landmark_data_proto = proto.mutable_landmark_data();
landmark_data_proto->set_trajectory_id(observation.trajectory_id);
landmark_data_proto->mutable_landmark_data()->set_timestamp(
common::ToUniversal(observation.time));
auto* observation_proto = landmark_data_proto->mutable_landmark_data()
->add_landmark_observations();
observation_proto->set_id(node.first);
*observation_proto->mutable_landmark_to_tracking_transform() =
transform::ToProto(observation.landmark_to_tracking_transform);
observation_proto->set_translation_weight(
observation.translation_weight);
observation_proto->set_rotation_weight(observation.rotation_weight);
writer->WriteProto(proto);
}
}
}
}
void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,

4
cartographer/mapping/pose_graph.h
View File

@ -129,6 +129,10 @@ class PoseGraph : public PoseGraphInterface {
virtual sensor::MapByTime<sensor::FixedFramePoseData>
GetFixedFramePoseData() = 0;
// Returns the landmark data.
virtual std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() = 0;
// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
// respect to 'to_trajectory_id' at time 'time'.
virtual void SetInitialTrajectoryPose(int from_trajectory_id,