Deserialize landmark poses. (#1064)

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -109,6 +109,11 @@ std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses() {
   return landmark_poses;
 }
 
+void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id,
+                                    const transform::Rigid3d& global_pose) {
+  LOG(FATAL) << "Not implemented";
+}
+
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
   proto::IsTrajectoryFinishedRequest request;
   request.set_trajectory_id(trajectory_id);

cartographer/cloud/internal/client/pose_graph_stub.h

@@ -39,6 +39,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
   GetTrajectoryNodePoses() override;
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override;
+  void SetLandmarkPose(const std::string& landmark_id,
+                       const transform::Rigid3d& global_pose) override;
   bool IsTrajectoryFinished(int trajectory_id) override;
   bool IsTrajectoryFrozen(int trajectory_id) override;
   std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -627,6 +627,14 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() {
   return landmark_poses;
 }
 
+void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
+                                  const transform::Rigid3d& global_pose) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([=]() REQUIRES(mutex_) {
+    landmark_nodes_[landmark_id].global_landmark_pose = global_pose;
+  });
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_->imu_data();

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

@@ -121,6 +121,9 @@ class PoseGraph2D : public PoseGraph {
       EXCLUDES(mutex_);
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
       EXCLUDES(mutex_);
+  void SetLandmarkPose(const std::string& landmark_id,
+                       const transform::Rigid3d& global_pose) override
+      EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -669,6 +669,14 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses() {
   return landmark_poses;
 }
 
+void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id,
+                                  const transform::Rigid3d& global_pose) {
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([=]() REQUIRES(mutex_) {
+    landmark_nodes_[landmark_id].global_landmark_pose = global_pose;
+  });
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_->imu_data();

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -120,6 +120,9 @@ class PoseGraph3D : public PoseGraph {
       EXCLUDES(mutex_);
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
       EXCLUDES(mutex_);
+  void SetLandmarkPose(const std::string& landmark_id,
+                       const transform::Rigid3d& global_pose) override
+      EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);

cartographer/mapping/internal/3d/pose_graph_3d_test.cc

@@ -26,23 +26,22 @@
 
 namespace cartographer {
 namespace mapping {
-namespace pose_graph {
-
-class FakeOptimizationProblem3D : public OptimizationProblem3D {
- public:
-  FakeOptimizationProblem3D()
-      : OptimizationProblem3D(proto::OptimizationProblemOptions{}) {}
-  ~FakeOptimizationProblem3D() override = default;
-  void Solve(
-      const std::vector<Constraint>& constraints,
-      const std::set<int>& frozen_trajectories,
-      const std::map<std::string, LandmarkNode>& landmark_nodes) override {}
-};
-
-}  // namespace pose_graph
-
 namespace {
 
+using ::cartographer::mapping::pose_graph::OptimizationProblem3D;
+using ::cartographer::mapping::pose_graph::proto::OptimizationProblemOptions;
+using ::cartographer::transform::Rigid3d;
+
+class MockOptimizationProblem3D : public OptimizationProblem3D {
+ public:
+  MockOptimizationProblem3D()
+      : OptimizationProblem3D(OptimizationProblemOptions{}) {}
+  ~MockOptimizationProblem3D() override = default;
+
+  MOCK_METHOD3(Solve, void(const std::vector<Constraint>&, const std::set<int>&,
+                           const std::map<std::string, LandmarkNode>&));
+};
+
 class PoseGraph3DForTesting : public PoseGraph3D {
  public:
   PoseGraph3DForTesting(
@@ -78,7 +77,7 @@ class PoseGraph3DTest : public ::testing::Test {
 
   void BuildPoseGraphWithFakeOptimization() {
     auto optimization_problem =
-        common::make_unique<pose_graph::FakeOptimizationProblem3D>();
+        common::make_unique<MockOptimizationProblem3D>();
     pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
         pose_graph_options_, std::move(optimization_problem),
         thread_pool_.get());
@@ -109,14 +108,16 @@ TEST_F(PoseGraph3DTest, BasicSerialization) {
   proto::PoseGraph proto;
   auto fake_node = test::CreateFakeNode();
   test::AddToProtoGraph(fake_node, &proto);
-  pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), fake_node);
+  pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node);
   auto fake_submap = test::CreateFakeSubmap3D();
   test::AddToProtoGraph(fake_submap, &proto);
-  pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), fake_submap);
+  pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap);
   test::AddToProtoGraph(test::CreateFakeConstraint(fake_node, fake_submap),
                         &proto);
   pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
-
+  test::AddToProtoGraph(
+      test::CreateFakeLandmark("landmark_id", Rigid3d::Identity()), &proto);
+  pose_graph_->SetLandmarkPose("landmark_id", Rigid3d::Identity());
   pose_graph_->WaitForAllComputations();
   proto::PoseGraph actual_proto = pose_graph_->ToProto();
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
@@ -127,6 +128,8 @@ TEST_F(PoseGraph3DTest, BasicSerialization) {
       proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0)));
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
       proto.trajectory(0), actual_proto.trajectory(0)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.landmarks(0), actual_proto.landmarks(0)));
   EXPECT_TRUE(
       google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto));
 }
@@ -140,11 +143,11 @@ TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) {
   for (int i = 0; i < num_submaps_to_create; ++i) {
     int submap_index = (i < 3) ? 42 + i : 100 + i;
     auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index);
-    pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), submap);
+    pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap);
     for (int j = 0; j < num_nodes_per_submap; ++j) {
       int node_index = 7 + num_nodes_per_submap * submap_index + j;
       auto node = test::CreateFakeNode(trajectory_id, node_index);
-      pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), node);
+      pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node);
       proto::PoseGraph proto;
       auto constraint = test::CreateFakeConstraint(node, submap);
       // TODO(gaschler): Also remove inter constraints when all references are
@@ -219,11 +222,11 @@ TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) {
   for (int i = 0; i < num_submaps_to_create; ++i) {
     int submap_index = 42 + i;
     auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index);
-    pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), submap);
+    pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap);
     for (int j = 0; j < num_nodes_per_submap; ++j) {
       int node_index = 7 + num_nodes_per_submap * i + j;
       auto node = test::CreateFakeNode(trajectory_id, node_index);
-      pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), node);
+      pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node);
       proto::PoseGraph proto;
       auto constraint = test::CreateFakeConstraint(node, submap);
       constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP);

cartographer/mapping/internal/test_helpers.cc

@@ -120,6 +120,14 @@ proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id,
   return trajectory;
 }
 
+proto::PoseGraph::Landmark CreateFakeLandmark(
+    const std::string& landmark_id, const transform::Rigid3d& global_pose) {
+  proto::PoseGraph::Landmark landmark;
+  landmark.set_landmark_id(landmark_id);
+  *landmark.mutable_global_pose() = transform::ToProto(global_pose);
+  return landmark;
+}
+
 void AddToProtoGraph(const proto::Node& node_data,
                      proto::PoseGraph* pose_graph) {
   auto* trajectory =
@@ -148,6 +156,11 @@ void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint,
   *pose_graph->add_constraint() = constraint;
 }
 
+void AddToProtoGraph(const proto::PoseGraph::Landmark& landmark,
+                     proto::PoseGraph* pose_graph) {
+  *pose_graph->add_landmarks() = landmark;
+}
+
 }  // namespace test
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/test_helpers.h

@@ -43,6 +43,8 @@ proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node& node,
 
 proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id,
                                             proto::PoseGraph* pose_graph);
+proto::PoseGraph::Landmark CreateFakeLandmark(
+    const std::string& landmark_id, const transform::Rigid3d& global_pose);
 
 void AddToProtoGraph(const proto::Node& node_data,
                      proto::PoseGraph* pose_graph);
@@ -53,6 +55,9 @@ void AddToProtoGraph(const proto::Submap& submap_data,
 void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint,
                      proto::PoseGraph* pose_graph);
 
+void AddToProtoGraph(const proto::PoseGraph::Landmark& landmark_node,
+                     proto::PoseGraph* pose_graph);
+
 }  // namespace test
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/testing/mock_pose_graph.h

@@ -43,6 +43,8 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
       GetTrajectoryNodePoses,
       mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>());
   MOCK_METHOD0(GetLandmarkPoses, std::map<std::string, transform::Rigid3d>());
+  MOCK_METHOD2(SetLandmarkPose,
+               void(const std::string&, const transform::Rigid3d&));
   MOCK_METHOD1(IsTrajectoryFinished, bool(int));
   MOCK_METHOD1(IsTrajectoryFrozen, bool(int));
   MOCK_METHOD0(GetTrajectoryData,

cartographer/mapping/map_builder.cc

@@ -366,6 +366,12 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
     }
   }
 
+  // Set global poses of landmarks.
+  for (const auto& landmark : pose_graph_proto.landmarks()) {
+    pose_graph_->SetLandmarkPose(landmark.landmark_id(),
+                                 transform::ToRigid3(landmark.global_pose()));
+  }
+
   for (;;) {
     proto::SerializedData proto;
     if (!reader->ReadProto(&proto)) {

cartographer/mapping/pose_graph.h

@@ -133,6 +133,10 @@ class PoseGraph : public PoseGraphInterface {
   virtual std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
   GetLandmarkNodes() = 0;
 
+  // Sets global pose of landmark 'landmark_id' to given 'global_pose'.
+  virtual void SetLandmarkPose(const std::string& landmark_id,
+                               const transform::Rigid3d& global_pose) = 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,

cartographer/mapping/pose_graph_interface.h

@@ -108,6 +108,10 @@ class PoseGraphInterface {
   // Returns the current optimized landmark poses.
   virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses() = 0;
 
+  // Sets global pose of landmark 'landmark_id' to given 'global_pose'.
+  virtual void SetLandmarkPose(const std::string& landmark_id,
+                               const transform::Rigid3d& global_pose) = 0;
+
   // Checks if the given trajectory is finished.
   virtual bool IsTrajectoryFinished(int trajectory_id) = 0;