Propagate LandmarksData to the PoseGraph. (#830)

Propagate LandmarkData to the PoseGraph.

cartographer/internal/mapping/global_trajectory_builder.h

@@ -106,6 +106,11 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
     pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
   }
 
+  void AddSensorData(const std::string& sensor_id,
+                     const sensor::LandmarkData& landmark_data) override {
+    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
+  }
+
   void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
                                   local_slam_result_data) override {
     CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "

cartographer/mapping/collated_trajectory_builder.h

@@ -69,6 +69,12 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
       const sensor::FixedFramePoseData& fixed_frame_pose_data) override {
     AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
   }
+
+  void AddSensorData(const std::string& sensor_id,
+                     const sensor::LandmarkData& landmark_data) override {
+    AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
+  }
+
   void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
                                   local_slam_result_data) override {
     AddData(std::move(local_slam_result_data));

cartographer/mapping/pose_graph.h

@@ -34,6 +34,7 @@
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
+#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"
@@ -71,6 +72,10 @@ class PoseGraph : public PoseGraphInterface {
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data) = 0;
 
+  // Inserts landmarks observations.
+  virtual void AddLandmarkData(int trajectory_id,
+                               const sensor::LandmarkData& landmark_data) = 0;
+
   // Finishes the given trajectory.
   virtual void FinishTrajectory(int trajectory_id) = 0;
 

cartographer/mapping/trajectory_builder_interface.h

@@ -29,6 +29,7 @@
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/landmark_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/timed_point_cloud_data.h"
 
@@ -78,7 +79,8 @@ class TrajectoryBuilderInterface {
   virtual void AddSensorData(
       const std::string& sensor_id,
       const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
-
+  virtual void AddSensorData(const std::string& sensor_id,
+                             const sensor::LandmarkData& landmark_data) = 0;
   // Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
   // is invalid to add local SLAM results for a trajectory that has a
   // 'LocalTrajectoryBuilder'.

cartographer/mapping_2d/pose_graph.cc

@@ -172,6 +172,12 @@ void PoseGraph::AddFixedFramePoseData(
   LOG(FATAL) << "Not yet implemented for 2D.";
 }
 
+void PoseGraph::AddLandmarkData(int trajectory_id,
+                                const sensor::LandmarkData& landmark_data)
+    EXCLUDES(mutex_) {
+  LOG(FATAL) << "Not yet implemented.";
+}
+
 void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                   const mapping::SubmapId& submap_id) {
   CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

cartographer/mapping_2d/pose_graph.h

@@ -85,6 +85,9 @@ class PoseGraph : public mapping::PoseGraph {
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data) override
       EXCLUDES(mutex_);
+  void AddLandmarkData(int trajectory_id,
+                       const sensor::LandmarkData& landmark_data) override
+      EXCLUDES(mutex_);
 
   void FinishTrajectory(int trajectory_id) override;
   bool IsTrajectoryFinished(int trajectory_id) override;

cartographer/mapping_3d/pose_graph.cc

@@ -174,6 +174,12 @@ void PoseGraph::AddFixedFramePoseData(
   });
 }
 
+void PoseGraph::AddLandmarkData(int trajectory_id,
+                                const sensor::LandmarkData& landmark_data)
+    EXCLUDES(mutex_) {
+  LOG(FATAL) << "Not yet implemented.";
+}
+
 void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                   const mapping::SubmapId& submap_id) {
   CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

cartographer/mapping_3d/pose_graph.h

@@ -85,6 +85,9 @@ class PoseGraph : public mapping::PoseGraph {
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data) override
       EXCLUDES(mutex_);
+  void AddLandmarkData(int trajectory_id,
+                       const sensor::LandmarkData& landmark_data) override
+      EXCLUDES(mutex_);
 
   void FinishTrajectory(int trajectory_id) override;
   bool IsTrajectoryFinished(int trajectory_id) override;

cartographer_grpc/client_server_test.cc

@@ -78,6 +78,8 @@ class MockTrajectoryBuilder
   MOCK_METHOD2(AddSensorData,
                void(const std::string&,
                     const cartographer::sensor::FixedFramePoseData&));
+  MOCK_METHOD2(AddSensorData, void(const std::string&,
+                                   const cartographer::sensor::LandmarkData&));
 
   // Some of the platforms we run on may ship with a version of gmock which does
   // not yet support move-only types.

cartographer_grpc/mapping/trajectory_builder_stub.cc

@@ -127,6 +127,12 @@ void TrajectoryBuilderStub::AddSensorData(
   fixed_frame_writer_.client_writer->Write(request);
 }
 
+void TrajectoryBuilderStub::AddSensorData(
+    const std::string& sensor_id,
+    const cartographer::sensor::LandmarkData& landmark_data) {
+  LOG(FATAL) << "Not implemented";
+}
+
 void TrajectoryBuilderStub::AddLocalSlamResultData(
     std::unique_ptr<cartographer::mapping::LocalSlamResultData>
         local_slam_result_data) {

cartographer_grpc/mapping/trajectory_builder_stub.h

@@ -49,6 +49,9 @@ class TrajectoryBuilderStub
   void AddSensorData(const std::string& sensor_id,
                      const cartographer::sensor::FixedFramePoseData&
                          fixed_frame_pose) override;
+  void AddSensorData(
+      const std::string& sensor_id,
+      const cartographer::sensor::LandmarkData& landmark_data) override;
   void AddLocalSlamResultData(
       std::unique_ptr<cartographer::mapping::LocalSlamResultData>
           local_slam_result_data) override;