From 7964211fef53b13831a09baaa022fdce80f6c19e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 2 Nov 2017 22:01:01 +0100 Subject: [PATCH] Deserialize SPG constraints (#544) --- cartographer/mapping/sparse_pose_graph.cc | 34 ++++++++++++++++++++ cartographer/mapping/sparse_pose_graph.h | 10 ++++++ cartographer/mapping_2d/sparse_pose_graph.cc | 27 ++++++++++++++++ cartographer/mapping_2d/sparse_pose_graph.h | 2 ++ cartographer/mapping_3d/sparse_pose_graph.cc | 20 ++++++++++++ cartographer/mapping_3d/sparse_pose_graph.h | 2 ++ 6 files changed, 95 insertions(+) diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index f89948a..f933fdc 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -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 FromProto( + const ::google::protobuf::RepeatedPtrField< + ::cartographer::mapping::proto::SparsePoseGraph::Constraint>& + constraint_protos) { + std::vector 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; diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 576410b..e5f0ca6 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -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& 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 trimmer) = 0; @@ -120,6 +125,11 @@ class SparsePoseGraph { virtual std::vector constraints() = 0; }; +std::vector FromProto( + const ::google::protobuf::RepeatedPtrField< + ::cartographer::mapping::proto::SparsePoseGraph::Constraint>& + constraint_protos); + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index ad9c0e2..8ba549e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -427,6 +427,33 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, }); } +void SparsePoseGraph::AddSerializedConstraints( + const std::vector& 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 trimmer) { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index f370380..2368318 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -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& constraints) override; void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index ed79a71..ce0bb18 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -441,6 +441,26 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, }); } +void SparsePoseGraph::AddSerializedConstraints( + const std::vector& 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 trimmer) { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index fddd023..039b4fe 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -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& constraints) override; void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override;