Deserialize SPG constraints (#544)

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<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;

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<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
 

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<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_);

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<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;

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<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_);

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<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;