Implement GetLandmarkPoses method. (#888)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)

cartographer/mapping/pose_graph.h

@@ -37,7 +37,6 @@
 #include "cartographer/sensor/landmark_data.h"
 #include "cartographer/sensor/map_by_time.h"
 #include "cartographer/sensor/odometry_data.h"
-#include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping {

cartographer/mapping/pose_graph_interface.h

@@ -22,6 +22,7 @@
 #include "cartographer/common/optional.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/submaps.h"
+#include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping {
@@ -98,6 +99,9 @@ class PoseGraphInterface {
   // Returns the current optimized trajectory poses.
   virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() = 0;
 
+  // Returns the current optimized landmark poses.
+  virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses() = 0;
+
   // Checks if the given trajectory is finished.
   virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
 

cartographer/mapping_2d/pose_graph.cc

@@ -606,6 +606,17 @@ PoseGraph::GetTrajectoryNodePoses() {
   return node_poses;
 }
 
+std::map<std::string, transform::Rigid3d> PoseGraph::GetLandmarkPoses() {
+  std::map<std::string, transform::Rigid3d> landmark_poses;
+  for (const auto& landmark : landmark_nodes_) {
+    // Landmark without value has not been optimized yet.
+    if (!landmark.second.global_landmark_pose.has_value()) continue;
+    landmark_poses[landmark.first] =
+        landmark.second.global_landmark_pose.value();
+  }
+  return landmark_poses;
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_.imu_data();

cartographer/mapping_2d/pose_graph.h

@@ -116,6 +116,8 @@ class PoseGraph : public mapping::PoseGraph {
   GetTrajectoryNodes() override EXCLUDES(mutex_);
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
   GetTrajectoryNodePoses() override EXCLUDES(mutex_);
+  std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
+      EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);

cartographer/mapping_3d/pose_graph.cc

@@ -635,6 +635,17 @@ PoseGraph::GetTrajectoryNodePoses() {
   return node_poses;
 }
 
+std::map<std::string, transform::Rigid3d> PoseGraph::GetLandmarkPoses() {
+  std::map<std::string, transform::Rigid3d> landmark_poses;
+  for (const auto& landmark : landmark_nodes_) {
+    // Landmark without value has not been optimized yet.
+    if (!landmark.second.global_landmark_pose.has_value()) continue;
+    landmark_poses[landmark.first] =
+        landmark.second.global_landmark_pose.value();
+  }
+  return landmark_poses;
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_.imu_data();

cartographer/mapping_3d/pose_graph.h

@@ -42,7 +42,6 @@
 #include "cartographer/sensor/landmark_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/point_cloud.h"
-#include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 
 namespace cartographer {
@@ -116,6 +115,8 @@ class PoseGraph : public mapping::PoseGraph {
   GetTrajectoryNodes() override EXCLUDES(mutex_);
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
   GetTrajectoryNodePoses() override EXCLUDES(mutex_);
+  std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
+      EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);

cartographer_grpc/handlers/get_landmark_poses_handler.cc

@@ -0,0 +1,46 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_grpc/handlers/get_landmark_poses_handler.h"
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer_grpc/framework/rpc_handler.h"
+#include "cartographer_grpc/map_builder_context_interface.h"
+#include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+
+void GetLandmarkPosesHandler::OnRequest(
+    const google::protobuf::Empty& request) {
+  auto landmark_poses = GetContext<MapBuilderContextInterface>()
+                            ->map_builder()
+                            .pose_graph()
+                            ->GetLandmarkPoses();
+  auto response =
+      cartographer::common::make_unique<proto::GetLandmarkPosesResponse>();
+  for (const auto& landmark_pose : landmark_poses) {
+    auto* landmark = response->add_landmark_poses();
+    landmark->set_landmark_id(landmark_pose.first);
+    *landmark->mutable_global_pose() =
+        cartographer::transform::ToProto(landmark_pose.second);
+  }
+  Send(std::move(response));
+}
+
+}  // namespace handlers
+}  // namespace cartographer_grpc

cartographer_grpc/handlers/get_landmark_poses_handler.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_GRPC_HANDLERS_GET_LANDMARK_POSES_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_GET_LANDMARK_POSES_HANDLER_H
+
+#include "cartographer_grpc/framework/rpc_handler.h"
+#include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+
+class GetLandmarkPosesHandler
+    : public framework::RpcHandler<google::protobuf::Empty,
+                                   proto::GetLandmarkPosesResponse> {
+ public:
+  std::string method_name() const override {
+    return "/cartographer_grpc.proto.MapBuilderService/GetLandmarkPoses";
+  }
+  void OnRequest(const google::protobuf::Empty& request) override;
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_GET_LANDMARK_POSES_HANDLER_H

cartographer_grpc/handlers/get_landmark_poses_handler_test.cc

@@ -0,0 +1,81 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_grpc/handlers/get_landmark_poses_handler.h"
+#include "cartographer_grpc/testing/handler_test.h"
+#include "cartographer_grpc/testing/test_helpers.h"
+#include "google/protobuf/text_format.h"
+#include "gtest/gtest.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+namespace {
+
+using ::cartographer::transform::Rigid3d;
+using ::testing::_;
+using ::testing::Eq;
+using ::testing::Pointee;
+using ::testing::Truly;
+
+const std::string kMessage = R"PROTO(
+  landmark_poses {
+    landmark_id: "landmark_1"
+    global_pose {
+      translation {
+        x: 1 y: 2 z: 3
+      }
+      rotation {
+        w:1 x: 0 y: 0 z: 0
+      }
+    }
+  }
+  landmark_poses {
+    landmark_id: "landmark_2"
+    global_pose {
+      translation {
+        x: 3 y: 2 z: 1
+      }
+      rotation {
+        w:0 x: 1 y: 0 z: 0
+      }
+    }
+  }
+)PROTO";
+
+using GetLandmarkPosesHandlerTest =
+    testing::HandlerTest<GetLandmarkPosesHandler>;
+
+TEST_F(GetLandmarkPosesHandlerTest, NoLocalSlamUploader) {
+  std::map<std::string, Rigid3d> landmark_poses{
+      {"landmark_1", Rigid3d(Eigen::Vector3d(1., 2., 3.),
+                             Eigen::Quaterniond(1., 0., 0., 0.))},
+      {"landmark_2", Rigid3d(Eigen::Vector3d(3., 2., 1.),
+                             Eigen::Quaterniond(0., 1., 0., 0.))}};
+  EXPECT_CALL(*mock_pose_graph_, GetLandmarkPoses())
+      .WillOnce(::testing::Return(landmark_poses));
+  test_server_->SendWrite(google::protobuf::Empty());
+
+  proto::GetLandmarkPosesResponse expected_response;
+  EXPECT_TRUE(google::protobuf::TextFormat::ParseFromString(
+      kMessage, &expected_response));
+  EXPECT_THAT(
+      test_server_->response(),
+      ::testing::Truly(testing::BuildProtoPredicateEquals(&expected_response)));
+}
+
+}  // namespace
+}  // namespace handlers
+}  // namespace cartographer_grpc

cartographer_grpc/map_builder_server.cc

@@ -26,6 +26,7 @@
 #include "cartographer_grpc/handlers/finish_trajectory_handler.h"
 #include "cartographer_grpc/handlers/get_all_submap_poses.h"
 #include "cartographer_grpc/handlers/get_constraints_handler.h"
+#include "cartographer_grpc/handlers/get_landmark_poses_handler.h"
 #include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h"
 #include "cartographer_grpc/handlers/get_submap_handler.h"
 #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h"
@@ -70,6 +71,7 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler>();
   server_builder.RegisterHandler<handlers::GetSubmapHandler>();
   server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler>();
+  server_builder.RegisterHandler<handlers::GetLandmarkPosesHandler>();
   server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>();
   server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();
   server_builder.RegisterHandler<handlers::GetConstraintsHandler>();

cartographer_grpc/mapping/pose_graph_stub.cc

@@ -19,6 +19,7 @@
 #include "cartographer_grpc/framework/client.h"
 #include "cartographer_grpc/handlers/get_all_submap_poses.h"
 #include "cartographer_grpc/handlers/get_constraints_handler.h"
+#include "cartographer_grpc/handlers/get_landmark_poses_handler.h"
 #include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h"
 #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h"
 #include "cartographer_grpc/handlers/run_final_optimization_handler.h"
@@ -103,6 +104,19 @@ PoseGraphStub::GetTrajectoryNodePoses() {
   return node_poses;
 }
 
+std::map<std::string, cartographer::transform::Rigid3d>
+PoseGraphStub::GetLandmarkPoses() {
+  google::protobuf::Empty request;
+  framework::Client<handlers::GetLandmarkPosesHandler> client(client_channel_);
+  CHECK(client.Write(request));
+  std::map<std::string, cartographer::transform::Rigid3d> landmark_poses;
+  for (const auto& landmark_pose : client.response().landmark_poses()) {
+    landmark_poses[landmark_pose.landmark_id()] =
+        cartographer::transform::ToRigid3(landmark_pose.global_pose());
+  }
+  return landmark_poses;
+}
+
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
   LOG(FATAL) << "Not implemented";
 }

cartographer_grpc/mapping/pose_graph_stub.h

@@ -43,6 +43,8 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
   cartographer::mapping::MapById<cartographer::mapping::NodeId,
                                  cartographer::mapping::TrajectoryNodePose>
   GetTrajectoryNodePoses() override;
+  std::map<std::string, cartographer::transform::Rigid3d> GetLandmarkPoses()
+      override;
   bool IsTrajectoryFinished(int trajectory_id) override;
   std::vector<Constraint> constraints() override;
   cartographer::mapping::proto::PoseGraph ToProto() override;

cartographer_grpc/proto/map_builder_service.proto

@@ -124,6 +124,15 @@ message GetTrajectoryNodePosesResponse {
   repeated TrajectoryNodePose node_poses = 1;
 }
 
+message LandmarkPose {
+  string landmark_id = 1;
+  cartographer.transform.proto.Rigid3d global_pose = 2;
+}
+
+message GetLandmarkPosesResponse {
+  repeated LandmarkPose landmark_poses = 1;
+}
+
 message SubmapPose {
   cartographer.mapping.proto.SubmapId submap_id = 1;
   int32 submap_version = 2;
@@ -202,6 +211,10 @@ service MapBuilderService {
   rpc GetTrajectoryNodePoses(google.protobuf.Empty)
       returns (GetTrajectoryNodePosesResponse);
 
+  // Returns the current optimized landmark poses.
+  rpc GetLandmarkPoses(google.protobuf.Empty)
+      returns (GetLandmarkPosesResponse);
+
   // Returns the current optimized submap poses.
   rpc GetAllSubmapPoses(google.protobuf.Empty)
       returns (GetAllSubmapPosesResponse);

cartographer_grpc/testing/handler_test.h

@@ -20,7 +20,9 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer_grpc/framework/testing/rpc_handler_test_server.h"
 #include "cartographer_grpc/testing/mock_local_trajectory_uploader.h"
+#include "cartographer_grpc/testing/mock_map_builder.h"
 #include "cartographer_grpc/testing/mock_map_builder_context.h"
+#include "cartographer_grpc/testing/mock_pose_graph.h"
 #include "gtest/gtest.h"
 
 namespace cartographer_grpc {
@@ -41,6 +43,15 @@ class HandlerTest : public Test {
             ->template GetUnsynchronizedContext<MockMapBuilderContext>();
     mock_local_trajectory_uploader_ =
         cartographer::common::make_unique<MockLocalTrajectoryUploader>();
+    mock_map_builder_ = cartographer::common::make_unique<MockMapBuilder>();
+    mock_pose_graph_ = cartographer::common::make_unique<MockPoseGraph>();
+
+    EXPECT_CALL(*mock_map_builder_context_, map_builder())
+        .Times(::testing::AnyNumber())
+        .WillRepeatedly(::testing::ReturnPointee(mock_map_builder_.get()));
+    EXPECT_CALL(*mock_map_builder_, pose_graph())
+        .Times(::testing::AnyNumber())
+        .WillRepeatedly(Return(mock_pose_graph_.get()));
   }
 
   void SetNoLocalTrajectoryUploader() {
@@ -58,6 +69,8 @@ class HandlerTest : public Test {
       test_server_;
   MockMapBuilderContext* mock_map_builder_context_;
   std::unique_ptr<MockLocalTrajectoryUploader> mock_local_trajectory_uploader_;
+  std::unique_ptr<MockMapBuilder> mock_map_builder_;
+  std::unique_ptr<MockPoseGraph> mock_pose_graph_;
 };
 
 }  // namespace testing

cartographer_grpc/testing/mock_pose_graph.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_GRPC_TESTING_MOCK_POSE_GRAPH_H
+#define CARTOGRAPHER_GRPC_TESTING_MOCK_POSE_GRAPH_H
+
+#include "cartographer/mapping/pose_graph_interface.h"
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer_grpc {
+namespace testing {
+
+class MockPoseGraph : public cartographer::mapping::PoseGraphInterface {
+ public:
+  MockPoseGraph() = default;
+  ~MockPoseGraph() override = default;
+
+  MOCK_METHOD0(RunFinalOptimization, void());
+  MOCK_METHOD0(GetAllSubmapData,
+               cartographer::mapping::MapById<cartographer::mapping::SubmapId,
+                                              SubmapData>());
+  MOCK_METHOD0(GetAllSubmapPoses,
+               cartographer::mapping::MapById<cartographer::mapping::SubmapId,
+                                              SubmapPose>());
+  MOCK_METHOD1(GetLocalToGlobalTransform,
+               cartographer::transform::Rigid3d(int));
+  MOCK_METHOD0(
+      GetTrajectoryNodes,
+      cartographer::mapping::MapById<cartographer::mapping::NodeId,
+                                     cartographer::mapping::TrajectoryNode>());
+  MOCK_METHOD0(GetTrajectoryNodePoses,
+               cartographer::mapping::MapById<
+                   cartographer::mapping::NodeId,
+                   cartographer::mapping::TrajectoryNodePose>());
+  MOCK_METHOD0(GetLandmarkPoses,
+               std::map<std::string, cartographer::transform::Rigid3d>());
+  MOCK_METHOD1(IsTrajectoryFinished, bool(int));
+  MOCK_METHOD0(constraints, std::vector<Constraint>());
+  MOCK_METHOD0(ToProto, cartographer::mapping::proto::PoseGraph());
+};
+
+}  // namespace testing
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_TESTING_MOCK_POSE_GRAPH_H