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

Deserialize SPG constraints (#544)

master
Juraj Oršulić 2017-11-02 22:01:01 +01:00 committed by Christoph Schütte
parent 3c5a7aa2d8
commit 7964211fef
6 changed files with 95 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cartographer/mapping/sparse_pose_graph.cc
View File

@ -35,6 +35,40 @@ proto::SparsePoseGraph::Constraint::Tag ToProto(
LOG(FATAL) << "Unsupported tag.";
}
SparsePoseGraph::Constraint::Tag FromProto(
const proto::SparsePoseGraph::Constraint::Tag& proto) {
switch (proto) {
case proto::SparsePoseGraph::Constraint::INTRA_SUBMAP:
return SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP;
case proto::SparsePoseGraph::Constraint::INTER_SUBMAP:
return SparsePoseGraph::Constraint::Tag::INTER_SUBMAP;
}
LOG(FATAL) << "Unsupported tag.";
}
std::vector<SparsePoseGraph::Constraint> FromProto(
const ::google::protobuf::RepeatedPtrField<
::cartographer::mapping::proto::SparsePoseGraph::Constraint>&
constraint_protos) {
std::vector<SparsePoseGraph::Constraint> constraints;
for (const auto& constraint_proto : constraint_protos) {
const mapping::SubmapId submap_id{
constraint_proto.submap_id().trajectory_id(),
constraint_proto.submap_id().submap_index()};
const mapping::NodeId node_id{constraint_proto.node_id().trajectory_id(),
constraint_proto.node_id().node_index()};
const SparsePoseGraph::Constraint::Pose pose{
transform::ToRigid3(constraint_proto.relative_pose()),
constraint_proto.translation_weight(),
constraint_proto.rotation_weight()};
const SparsePoseGraph::Constraint::Tag tag =
FromProto(constraint_proto.tag());
constraints.push_back(
SparsePoseGraph::Constraint{submap_id, node_id, pose, tag});
}
return constraints;
}
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::SparsePoseGraphOptions options;

10
cartographer/mapping/sparse_pose_graph.h
View File

@ -87,6 +87,11 @@ class SparsePoseGraph {
virtual void AddNodeFromProto(const transform::Rigid3d& global_pose,
const proto::Node& node) = 0;
// Adds serialized constraints. The corresponding trajectory nodes and submaps
// have to be deserialized before calling this function.
virtual void AddSerializedConstraints(
const std::vector<Constraint>& constraints) = 0;
// Adds a 'trimmer'. It will be used after all data added before it has been
// included in the pose graph.
virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;
@ -120,6 +125,11 @@ class SparsePoseGraph {
virtual std::vector<Constraint> constraints() = 0;
};
std::vector<SparsePoseGraph::Constraint> FromProto(
const ::google::protobuf::RepeatedPtrField<
::cartographer::mapping::proto::SparsePoseGraph::Constraint>&
constraint_protos);
} // namespace mapping
} // namespace cartographer

27
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -427,6 +427,33 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
});
}
void SparsePoseGraph::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() REQUIRES(mutex_) {
for (const auto& constraint : constraints) {
CHECK(trajectory_nodes_.Contains(constraint.node_id));
CHECK(submap_data_.Contains(constraint.submap_id));
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
CHECK(submap_data_.at(constraint.submap_id).submap != nullptr);
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
CHECK(submap_data_.at(constraint.submap_id)
.node_ids.emplace(constraint.node_id)
.second);
}
const Constraint::Pose pose = {
constraint.pose.zbar_ij *
transform::Rigid3d::Rotation(
trajectory_nodes_.at(constraint.node_id)
.constant_data->gravity_alignment.inverse()),
constraint.pose.translation_weight, constraint.pose.rotation_weight};
constraints_.push_back(Constraint{
constraint.submap_id, constraint.node_id, pose, constraint.tag});
}
LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
});
}
void SparsePoseGraph::AddTrimmer(
std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override;
void AddSerializedConstraints(
const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;

20
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -441,6 +441,26 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
});
}
void SparsePoseGraph::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() REQUIRES(mutex_) {
for (const auto& constraint : constraints) {
CHECK(trajectory_nodes_.Contains(constraint.node_id));
CHECK(submap_data_.Contains(constraint.submap_id));
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
CHECK(submap_data_.at(constraint.submap_id).submap != nullptr);
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
CHECK(submap_data_.at(constraint.submap_id)
.node_ids.emplace(constraint.node_id)
.second);
}
constraints_.push_back(constraint);
}
LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
});
}
void SparsePoseGraph::AddTrimmer(
std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);

2
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override;
void AddSerializedConstraints(
const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;