Adds CollatedTrajectoryBuilder. (#82)

cartographer/mapping/CMakeLists.txt

@@ -15,15 +15,32 @@
 add_subdirectory("proto")
 add_subdirectory("sparse_pose_graph")
 
-google_library(mapping_global_trajectory_builder_interface
+google_library(mapping_collated_trajectory_builder
+  USES_GLOG
   SRCS
-    global_trajectory_builder_interface.cc
+    collated_trajectory_builder.cc
+  HDRS
+    collated_trajectory_builder.h
+  DEPENDS
+    common_port
+    common_rate_timer
+    common_time
+    kalman_filter_pose_tracker
+    mapping_global_trajectory_builder_interface
+    mapping_submaps
+    mapping_trajectory_builder
+    sensor_collator
+    sensor_data
+)
+
+google_library(mapping_global_trajectory_builder_interface
   HDRS
     global_trajectory_builder_interface.h
   DEPENDS
     common_time
     kalman_filter_pose_tracker
     mapping_submaps
+    mapping_trajectory_builder
     sensor_laser
     sensor_point_cloud
 )
@@ -53,16 +70,14 @@ google_library(mapping_map_builder
     common_make_unique
     common_thread_pool
     mapping_2d_global_trajectory_builder
-    mapping_2d_local_trajectory_builder
     mapping_2d_sparse_pose_graph
-    mapping_2d_submaps
     mapping_3d_global_trajectory_builder
     mapping_3d_local_trajectory_builder_options
-    mapping_3d_proto_local_trajectory_builder_options
     mapping_3d_sparse_pose_graph
     mapping_global_trajectory_builder_interface
     mapping_proto_map_builder_options
     mapping_sparse_pose_graph
+    mapping_submaps
     mapping_trajectory_node
     sensor_laser
     sensor_voxel_filter
@@ -125,6 +140,20 @@ google_library(mapping_submaps
     transform_transform
 )
 
+google_library(mapping_trajectory_builder
+  HDRS
+    trajectory_builder.h
+  DEPENDS
+    common_make_unique
+    common_port
+    common_time
+    kalman_filter_pose_tracker
+    mapping_submaps
+    sensor_data
+    sensor_laser
+    sensor_point_cloud
+)
+
 google_library(mapping_trajectory_connectivity
   USES_GLOG
   SRCS

cartographer/mapping/collated_trajectory_builder.cc

@@ -0,0 +1,112 @@
+/*
+ * 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.
+ */
+
+#include "cartographer/mapping/collated_trajectory_builder.h"
+
+#include "cartographer/common/time.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+namespace {
+
+constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
+
+}  // namespace
+
+CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
+    sensor::Collator* const sensor_collator, const int trajectory_id,
+    const std::unordered_set<string>& expected_sensor_ids,
+    std::unique_ptr<GlobalTrajectoryBuilderInterface>
+        wrapped_trajectory_builder)
+    : sensor_collator_(sensor_collator),
+      trajectory_id_(trajectory_id),
+      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
+      last_logging_time_(std::chrono::steady_clock::now()) {
+  sensor_collator_->AddTrajectory(
+      trajectory_id, expected_sensor_ids,
+      [this](const string& sensor_id, std::unique_ptr<sensor::Data> data) {
+        HandleCollatedSensorData(sensor_id, std::move(data));
+      });
+}
+
+CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
+
+const Submaps* CollatedTrajectoryBuilder::submaps() const {
+  return wrapped_trajectory_builder_->submaps();
+}
+
+Submaps* CollatedTrajectoryBuilder::submaps() {
+  return wrapped_trajectory_builder_->submaps();
+}
+
+kalman_filter::PoseTracker* CollatedTrajectoryBuilder::pose_tracker() const {
+  return wrapped_trajectory_builder_->pose_tracker();
+}
+
+const TrajectoryBuilder::PoseEstimate&
+CollatedTrajectoryBuilder::pose_estimate() const {
+  return wrapped_trajectory_builder_->pose_estimate();
+}
+
+void CollatedTrajectoryBuilder::AddSensorData(
+    const string& sensor_id, std::unique_ptr<sensor::Data> data) {
+  sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data));
+}
+
+void CollatedTrajectoryBuilder::HandleCollatedSensorData(
+    const string& sensor_id, std::unique_ptr<sensor::Data> data) {
+  auto it = rate_timers_.find(sensor_id);
+  if (it == rate_timers_.end()) {
+    it = rate_timers_
+             .emplace(
+                 std::piecewise_construct, std::forward_as_tuple(sensor_id),
+                 std::forward_as_tuple(
+                     common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
+             .first;
+  }
+  it->second.Pulse(data->time);
+
+  if (std::chrono::steady_clock::now() - last_logging_time_ >
+      common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
+    for (const auto& pair : rate_timers_) {
+      LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
+    }
+    last_logging_time_ = std::chrono::steady_clock::now();
+  }
+
+  switch (data->type) {
+    case sensor::Data::Type::kImu:
+      wrapped_trajectory_builder_->AddImuData(data->time,
+                                              data->imu.linear_acceleration,
+                                              data->imu.angular_velocity);
+      return;
+
+    case sensor::Data::Type::kLaserFan:
+      wrapped_trajectory_builder_->AddLaserFan(data->time, data->laser_fan);
+      return;
+
+    case sensor::Data::Type::kOdometry:
+      wrapped_trajectory_builder_->AddOdometerPose(
+          data->time, data->odometry.pose, data->odometry.covariance);
+      return;
+  }
+  LOG(FATAL);
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/collated_trajectory_builder.h

@@ -0,0 +1,77 @@
+/*
+ * 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_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_
+#define CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_
+
+#include <chrono>
+#include <map>
+#include <memory>
+#include <string>
+#include <unordered_set>
+
+#include "cartographer/common/port.h"
+#include "cartographer/common/rate_timer.h"
+#include "cartographer/kalman_filter/pose_tracker.h"
+#include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping/submaps.h"
+#include "cartographer/mapping/trajectory_builder.h"
+#include "cartographer/sensor/collator.h"
+#include "cartographer/sensor/data.h"
+
+namespace cartographer {
+namespace mapping {
+
+// Handles collating sensor data using a sensor::Collator, then passing it on to
+// a mapping::GlobalTrajectoryBuilderInterface which is common for 2D and 3D.
+class CollatedTrajectoryBuilder : public TrajectoryBuilder {
+ public:
+  CollatedTrajectoryBuilder(
+      sensor::Collator* sensor_collator, int trajectory_id,
+      const std::unordered_set<string>& expected_sensor_ids,
+      std::unique_ptr<GlobalTrajectoryBuilderInterface>
+          wrapped_trajectory_builder);
+  ~CollatedTrajectoryBuilder() override;
+
+  CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete;
+  CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
+      delete;
+
+  const Submaps* submaps() const override;
+  Submaps* submaps() override;
+  kalman_filter::PoseTracker* pose_tracker() const override;
+  const PoseEstimate& pose_estimate() const override;
+
+  void AddSensorData(const string& sensor_id,
+                     std::unique_ptr<sensor::Data> data) override;
+
+ private:
+  void HandleCollatedSensorData(const string& sensor_id,
+                                std::unique_ptr<sensor::Data> data);
+
+  sensor::Collator* const sensor_collator_;
+  const int trajectory_id_;
+  std::unique_ptr<GlobalTrajectoryBuilderInterface> wrapped_trajectory_builder_;
+
+  // Time at which we last logged the rates of incoming sensor data.
+  std::chrono::steady_clock::time_point last_logging_time_;
+  std::map<string, common::RateTimer<>> rate_timers_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_

cartographer/mapping/global_trajectory_builder_interface.cc

@@ -1,35 +0,0 @@
-/*
- * 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.
- */
-
-#include "cartographer/mapping/global_trajectory_builder_interface.h"
-
-namespace cartographer {
-namespace mapping {
-
-GlobalTrajectoryBuilderInterface::PoseEstimate::PoseEstimate(
-    const common::Time time, const kalman_filter::PoseAndCovariance& prediction,
-    const kalman_filter::PoseAndCovariance& observation,
-    const kalman_filter::PoseAndCovariance& estimate,
-    const transform::Rigid3d& pose, const sensor::PointCloud& point_cloud)
-    : time(time),
-      prediction(prediction),
-      observation(observation),
-      estimate(estimate),
-      pose(pose),
-      point_cloud(point_cloud) {}
-
-}  // namespace mapping
-}  // namespace cartographer

cartographer/mapping/global_trajectory_builder_interface.h

@@ -24,6 +24,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/submaps.h"
+#include "cartographer/mapping/trajectory_builder.h"
 #include "cartographer/sensor/laser.h"
 #include "cartographer/sensor/point_cloud.h"
 
@@ -36,34 +37,7 @@ namespace mapping {
 // optimized pose estimates.
 class GlobalTrajectoryBuilderInterface {
  public:
-  // Represents a newly computed pose. Each of the following steps in the pose
+  using PoseEstimate = TrajectoryBuilder::PoseEstimate;
-  // estimation pipeline are provided for debugging:
-  //
-  // 1. UKF prediction
-  // 2. Absolute pose observation (e.g. from scan matching)
-  // 3. UKF estimate after integrating any measurements
-  //
-  // Finally, 'pose' is the end-user visualization of orientation and
-  // 'point_cloud' is the point cloud, in the local map frame.
-  struct PoseEstimate {
-    PoseEstimate() = default;
-    PoseEstimate(common::Time time,
-                 const kalman_filter::PoseAndCovariance& prediction,
-                 const kalman_filter::PoseAndCovariance& observation,
-                 const kalman_filter::PoseAndCovariance& estimate,
-                 const transform::Rigid3d& pose,
-                 const sensor::PointCloud& point_cloud);
-
-    common::Time time = common::Time::min();
-    kalman_filter::PoseAndCovariance prediction = {
-        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
-    kalman_filter::PoseAndCovariance observation = {
-        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
-    kalman_filter::PoseAndCovariance estimate = {
-        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
-    transform::Rigid3d pose = transform::Rigid3d::Identity();
-    sensor::PointCloud point_cloud;
-  };
 
   GlobalTrajectoryBuilderInterface() {}
   virtual ~GlobalTrajectoryBuilderInterface() {}

cartographer/mapping/map_builder.h

@@ -28,11 +28,9 @@
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
+#include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
-#include "cartographer/mapping_2d/local_trajectory_builder.h"
 #include "cartographer/mapping_2d/sparse_pose_graph.h"
-#include "cartographer/mapping_2d/submaps.h"
-#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/sparse_pose_graph.h"
 
 namespace cartographer {

cartographer/mapping/trajectory_builder.h

@@ -0,0 +1,114 @@
+/*
+ * 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_MAPPING_TRAJECTORY_BUILDER_H_
+#define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/pose_tracker.h"
+#include "cartographer/mapping/submaps.h"
+#include "cartographer/sensor/data.h"
+#include "cartographer/sensor/laser.h"
+#include "cartographer/sensor/point_cloud.h"
+
+namespace cartographer {
+namespace mapping {
+
+// This interface is used for both 2D and 3D SLAM.
+class TrajectoryBuilder {
+ public:
+  // Represents a newly computed pose. Each of the following steps in the pose
+  // estimation pipeline are provided for debugging:
+  //
+  // 1. UKF prediction
+  // 2. Absolute pose observation (e.g. from scan matching)
+  // 3. UKF estimate after integrating any measurements
+  //
+  // Finally, 'pose' is the end-user visualization of orientation and
+  // 'point_cloud' is the point cloud, in the local map frame.
+  struct PoseEstimate {
+    PoseEstimate() = default;
+    PoseEstimate(common::Time time,
+                 const kalman_filter::PoseAndCovariance& prediction,
+                 const kalman_filter::PoseAndCovariance& observation,
+                 const kalman_filter::PoseAndCovariance& estimate,
+                 const transform::Rigid3d& pose,
+                 const sensor::PointCloud& point_cloud)
+        : time(time),
+          prediction(prediction),
+          observation(observation),
+          estimate(estimate),
+          pose(pose),
+          point_cloud(point_cloud) {}
+
+    common::Time time = common::Time::min();
+    kalman_filter::PoseAndCovariance prediction = {
+        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
+    kalman_filter::PoseAndCovariance observation = {
+        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
+    kalman_filter::PoseAndCovariance estimate = {
+        transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
+    transform::Rigid3d pose = transform::Rigid3d::Identity();
+    sensor::PointCloud point_cloud;
+  };
+
+  TrajectoryBuilder() {}
+  virtual ~TrajectoryBuilder() {}
+
+  TrajectoryBuilder(const TrajectoryBuilder&) = delete;
+  TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
+
+  virtual const Submaps* submaps() const = 0;
+  virtual Submaps* submaps() = 0;
+  virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
+  virtual const PoseEstimate& pose_estimate() const = 0;
+
+  virtual void AddSensorData(const string& sensor_id,
+                             std::unique_ptr<sensor::Data> data) = 0;
+
+  void AddLaserFan(const string& sensor_id, common::Time time,
+                   const sensor::LaserFan& laser_fan) {
+    AddSensorData(sensor_id,
+                  common::make_unique<sensor::Data>(time, laser_fan));
+  }
+
+  void AddImuData(const string& sensor_id, common::Time time,
+                  const Eigen::Vector3d& linear_acceleration,
+                  const Eigen::Vector3d& angular_velocity) {
+    AddSensorData(sensor_id, common::make_unique<sensor::Data>(
+                                 time, sensor::Data::Imu{linear_acceleration,
+                                                         angular_velocity}));
+  }
+
+  void AddOdometerPose(const string& sensor_id, common::Time time,
+                       const transform::Rigid3d& pose,
+                       const kalman_filter::PoseCovariance& covariance) {
+    AddSensorData(sensor_id,
+                  common::make_unique<sensor::Data>(
+                      time, sensor::Data::Odometry{pose, covariance}));
+  }
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_

cartographer/mapping_2d/global_trajectory_builder.h

@@ -40,7 +40,8 @@ class GlobalTrajectoryBuilder
   const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
       const override;
 
-  // Handles approximately horizontal laser fans.
+  // Projects 'laser_fan' to 2D, and therefore should be approximately
+  // horizontal.
   void AddLaserFan(common::Time time,
                    const sensor::LaserFan& laser_fan) override;
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,