Implement LocalSlamResult data adding to PoseGraph. (#804)

cartographer/internal/mapping/global_trajectory_builder.h

@@ -19,6 +19,7 @@
 
 #include "cartographer/mapping/trajectory_builder_interface.h"
 
+#include "cartographer/mapping/local_slam_result_data.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -105,6 +106,13 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
     pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
   }
 
+  void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
+                                  local_slam_result_data) override {
+    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
+                                         "local_trajectory_builder_ present.";
+    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
+  }
+
  private:
   const int trajectory_id_;
   PoseGraph* const pose_graph_;

cartographer/mapping/collated_trajectory_builder.cc

@@ -45,8 +45,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
 
 CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
 
-void CollatedTrajectoryBuilder::AddSensorData(
+void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
-    std::unique_ptr<sensor::Data> data) {
   sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
 }
 

cartographer/mapping/collated_trajectory_builder.h

@@ -28,7 +28,7 @@
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer/sensor/collator.h"
-#include "cartographer/sensor/data.h"
+#include "cartographer/sensor/dispatchable.h"
 
 namespace cartographer {
 namespace mapping {
@@ -50,27 +50,31 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
   void AddSensorData(
       const std::string& sensor_id,
       const sensor::TimedPointCloudData& timed_point_cloud_data) override {
-    AddSensorData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
+    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
   }
 
   void AddSensorData(const std::string& sensor_id,
                      const sensor::ImuData& imu_data) override {
-    AddSensorData(sensor::MakeDispatchable(sensor_id, imu_data));
+    AddData(sensor::MakeDispatchable(sensor_id, imu_data));
   }
 
   void AddSensorData(const std::string& sensor_id,
                      const sensor::OdometryData& odometry_data) override {
-    AddSensorData(sensor::MakeDispatchable(sensor_id, odometry_data));
+    AddData(sensor::MakeDispatchable(sensor_id, odometry_data));
   }
 
   void AddSensorData(
       const std::string& sensor_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data) override {
-    AddSensorData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
+    AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
+  }
+  void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
+                                  local_slam_result_data) override {
+    AddData(std::move(local_slam_result_data));
   }
 
  private:
-  void AddSensorData(std::unique_ptr<sensor::Data> data);
+  void AddData(std::unique_ptr<sensor::Data> data);
 
   void HandleCollatedSensorData(const std::string& sensor_id,
                                 std::unique_ptr<sensor::Data> data);

cartographer/mapping/local_slam_result_data.cc

@@ -0,0 +1,64 @@
+/*
+ * 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 "local_slam_result_data.h"
+#include "cartographer/mapping/trajectory_builder_interface.h"
+
+namespace cartographer {
+namespace mapping {
+
+LocalSlamResultData::LocalSlamResultData(const std::string &sensor_id,
+                                         common::Time time)
+    : Data(sensor_id), time_(time) {}
+
+LocalSlamResult2D::LocalSlamResult2D(const std::string &sensor_id,
+                                     common::Time time)
+    : LocalSlamResultData(sensor_id, time) {}
+
+void LocalSlamResult2D::AddToTrajectoryBuilder(
+    mapping::TrajectoryBuilderInterface *const trajectory_builder) {
+  trajectory_builder->AddLocalSlamResultData(
+      common::make_unique<LocalSlamResult2D>(*this));
+}
+
+void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
+                                       mapping::PoseGraph *pose_graph) const {
+  DCHECK(dynamic_cast<mapping_2d::PoseGraph *>(pose_graph));
+  mapping_2d::PoseGraph *pose_graph_2d =
+      static_cast<mapping_2d::PoseGraph *>(pose_graph);
+  pose_graph_2d->AddNode(constant_data, trajectory_id, insertion_submaps);
+}
+
+LocalSlamResult3D::LocalSlamResult3D(const std::string &sensor_id,
+                                     common::Time time)
+    : LocalSlamResultData(sensor_id, time) {}
+
+void LocalSlamResult3D::AddToTrajectoryBuilder(
+    mapping::TrajectoryBuilderInterface *const trajectory_builder) {
+  trajectory_builder->AddLocalSlamResultData(
+      common::make_unique<LocalSlamResult3D>(*this));
+}
+
+void LocalSlamResult3D::AddToPoseGraph(int trajectory_id,
+                                       mapping::PoseGraph *pose_graph) const {
+  DCHECK(dynamic_cast<mapping_3d::PoseGraph *>(pose_graph));
+  mapping_3d::PoseGraph *pose_graph_3d =
+      static_cast<mapping_3d::PoseGraph *>(pose_graph);
+  pose_graph_3d->AddNode(constant_data, trajectory_id, insertion_submaps);
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/local_slam_result_data.h

@@ -0,0 +1,71 @@
+/*
+ * 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_MAPPING_LOCAL_SLAM_RESULT_DATA_H
+#define CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H
+
+#include "cartographer/mapping_2d/pose_graph.h"
+#include "cartographer/mapping_3d/pose_graph.h"
+#include "cartographer/sensor/data.h"
+
+namespace cartographer {
+namespace mapping {
+
+class TrajectoryBuilderInterface;
+class LocalSlamResultData : public cartographer::sensor::Data {
+ public:
+  LocalSlamResultData(const std::string &sensor_id, common::Time time);
+
+  common::Time GetTime() const override { return time_; }
+  virtual void AddToPoseGraph(int trajectory_id,
+                              mapping::PoseGraph *pose_graph) const = 0;
+
+ private:
+  common::Time time_;
+};
+
+class LocalSlamResult2D : public LocalSlamResultData {
+ public:
+  LocalSlamResult2D(const std::string &sensor_id, common::Time time);
+
+  void AddToTrajectoryBuilder(
+      mapping::TrajectoryBuilderInterface *const trajectory_builder) override;
+  void AddToPoseGraph(int trajectory_id,
+                      mapping::PoseGraph *pose_graph) const override;
+
+ private:
+  std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
+  std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps;
+};
+
+class LocalSlamResult3D : public LocalSlamResultData {
+ public:
+  LocalSlamResult3D(const std::string &sensor_id, common::Time time);
+
+  void AddToTrajectoryBuilder(
+      mapping::TrajectoryBuilderInterface *const trajectory_builder) override;
+  void AddToPoseGraph(int trajectory_id,
+                      mapping::PoseGraph *pose_graph) const override;
+
+ private:
+  std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
+  std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H

cartographer/mapping/trajectory_builder_interface.h

@@ -25,6 +25,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
@@ -76,6 +77,12 @@ class TrajectoryBuilderInterface {
   virtual void AddSensorData(
       const std::string& sensor_id,
       const sensor::FixedFramePoseData& fixed_frame_pose) = 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'.
+  virtual void AddLocalSlamResultData(
+      std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
 };
 
 }  // namespace mapping

cartographer/sensor/data.h

@@ -19,10 +19,14 @@
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
-#include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
+
+namespace mapping {
+class TrajectoryBuilderInterface;
+}
+
 namespace sensor {
 
 class Data {
@@ -39,28 +43,6 @@ class Data {
   const std::string sensor_id_;
 };
 
-template <typename DataType>
-class Dispatchable : public Data {
- public:
-  Dispatchable(const std::string &sensor_id, const DataType &data)
-      : Data(sensor_id), data_(data) {}
-
-  common::Time GetTime() const override { return data_.time; }
-  void AddToTrajectoryBuilder(
-      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
-    trajectory_builder->AddSensorData(sensor_id_, data_);
-  }
-
- private:
-  const DataType data_;
-};
-
-template <typename DataType>
-std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
-    const std::string &sensor_id, const DataType &data) {
-  return common::make_unique<Dispatchable<DataType>>(sensor_id, data);
-}
-
 }  // namespace sensor
 }  // namespace cartographer
 

cartographer/sensor/dispatchable.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright 2016 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_SENSOR_MAPPING_DISPATCHABLE_H_
+#define CARTOGRAPHER_SENSOR_MAPPING_DISPATCHABLE_H_
+
+#include "cartographer/mapping/trajectory_builder_interface.h"
+#include "cartographer/sensor/data.h"
+
+namespace cartographer {
+namespace sensor {
+
+template <typename DataType>
+class Dispatchable : public Data {
+ public:
+  Dispatchable(const std::string &sensor_id, const DataType &data)
+      : Data(sensor_id), data_(data) {}
+
+  common::Time GetTime() const override { return data_.time; }
+  void AddToTrajectoryBuilder(
+      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
+    trajectory_builder->AddSensorData(sensor_id_, data_);
+  }
+
+ private:
+  const DataType data_;
+};
+
+template <typename DataType>
+std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
+    const std::string &sensor_id, const DataType &data) {
+  return common::make_unique<Dispatchable<DataType>>(sensor_id, data);
+}
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_MAPPING_DISPATCHABLE_H_

cartographer/sensor/ordered_multi_queue.h

@@ -26,7 +26,7 @@
 #include "cartographer/common/blocking_queue.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
-#include "cartographer/sensor/data.h"
+#include "cartographer/sensor/dispatchable.h"
 
 namespace cartographer {
 namespace sensor {

cartographer_grpc/client_server_test.cc

@@ -18,6 +18,7 @@
 #include <mutex>
 
 #include "cartographer/internal/mapping/test_helpers.h"
+#include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer_grpc/map_builder_server.h"
 #include "cartographer_grpc/map_builder_server_options.h"
 #include "cartographer_grpc/mapping/map_builder_stub.h"
@@ -77,6 +78,16 @@ class MockTrajectoryBuilder
   MOCK_METHOD2(AddSensorData,
                void(const std::string&,
                     const cartographer::sensor::FixedFramePoseData&));
+
+  // Some of the platforms we run on may ship with a version of gmock which does
+  // not yet support move-only types.
+  MOCK_METHOD1(DoAddLocalSlamResultData,
+               void(cartographer::mapping::LocalSlamResultData*));
+  void AddLocalSlamResultData(
+      std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+          local_slam_result_data) override {
+    DoAddLocalSlamResultData(local_slam_result_data.get());
+  }
 };
 
 class ClientServerTest : public ::testing::Test {

cartographer_grpc/data_uploader.cc

@@ -0,0 +1,23 @@
+/*
+ * 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/data_uploader.h"
+
+namespace cartographer_grpc {
+
+DataUploader::DataUploader(const std::string &data_uploader) {}
+
+}  // namespace cartographer_grpc

cartographer_grpc/data_uploader.h

@@ -0,0 +1,31 @@
+/*
+ * 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_DATA_UPLOADER_H
+#define CARTOGRAPHER_GRPC_DATA_UPLOADER_H
+
+#include <string>
+
+namespace cartographer_grpc {
+
+class DataUploader {
+ public:
+  DataUploader(const std::string &server_address);
+};
+
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_DATA_UPLOADER_H

cartographer_grpc/map_builder_server.h

@@ -21,7 +21,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/map_builder.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
-#include "cartographer/sensor/data.h"
+#include "cartographer/sensor/dispatchable.h"
 #include "cartographer_grpc/framework/execution_context.h"
 #include "cartographer_grpc/framework/server.h"
 #include "cartographer_grpc/proto/map_builder_server_options.pb.h"

cartographer_grpc/mapping/trajectory_builder_stub.cc

@@ -16,6 +16,7 @@
 
 #include "cartographer_grpc/mapping/trajectory_builder_stub.h"
 
+#include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer_grpc/proto/map_builder_service.pb.h"
 #include "glog/logging.h"
 
@@ -124,6 +125,12 @@ void TrajectoryBuilderStub::AddSensorData(
   fixed_frame_writer_.client_writer->Write(request);
 }
 
+void TrajectoryBuilderStub::AddLocalSlamResultData(
+    std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+        local_slam_result_data) {
+  LOG(FATAL) << "Not implemented";
+}
+
 proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata(
     const std::string& sensor_id) {
   proto::SensorMetadata sensor_metadata;

cartographer_grpc/mapping/trajectory_builder_stub.h

@@ -19,6 +19,7 @@
 
 #include <thread>
 
+#include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
 #include "grpc++/grpc++.h"
@@ -47,6 +48,9 @@ class TrajectoryBuilderStub
   void AddSensorData(const std::string& sensor_id,
                      const cartographer::sensor::FixedFramePoseData&
                          fixed_frame_pose) override;
+  void AddLocalSlamResultData(
+      std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+          local_slam_result_data) override;
 
  private:
   template <typename RequestType>