TrajectoryCollator (#827)

Introduces TrajectorCollator, which collates sensor data ignoring
other trajectories.
Tests the same.

[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)

cartographer/sensor/test_helpers.h

@@ -20,6 +20,13 @@
 #include <string>
 #include <tuple>
 
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/collator_interface.h"
+#include "cartographer/sensor/dispatchable.h"
+#include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
+#include "cartographer/sensor/timed_point_cloud_data.h"
 #include "gmock/gmock.h"
 
 namespace cartographer {
@@ -29,6 +36,51 @@ MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") {
   return arg.isApprox(point, 0.001f);
 }
 
+namespace test {
+
+typedef std::tuple<int /* trajectory_id */, std::string /* sensor_id */,
+                   common::Time>
+    CollatorOutput;
+
+struct CollatorInput {
+  static CollatorInput CreateImuData(int trajectory_id,
+                                     const std::string& sensor_id, int time) {
+    return CollatorInput{
+        trajectory_id,
+        MakeDispatchable(sensor_id, ImuData{common::FromUniversal(time)}),
+        CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}};
+  }
+  static CollatorInput CreateTimedPointCloudData(int trajectory_id,
+                                                 const std::string& sensor_id,
+                                                 int time) {
+    return CollatorInput{
+        trajectory_id,
+        MakeDispatchable(
+            sensor_id,
+            TimedPointCloudData{
+                common::FromUniversal(time), Eigen::Vector3f::Zero(), {}}),
+        CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}};
+  }
+  static CollatorInput CreateOdometryData(int trajectory_id,
+                                          const std::string& sensor_id,
+                                          int time) {
+    return CollatorInput{
+        trajectory_id,
+        MakeDispatchable(sensor_id,
+                         OdometryData{common::FromUniversal(time),
+                                      transform::Rigid3d::Identity()}),
+        CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}};
+  }
+  void MoveToCollator(CollatorInterface* collator) {
+    collator->AddSensorData(trajectory_id, std::move(data));
+  }
+
+  const int trajectory_id;
+  std::unique_ptr<sensor::Data> data;
+  const CollatorOutput expected_output;
+};
+
+}  // namespace test
 }  // namespace sensor
 }  // namespace cartographer
 

cartographer/sensor/trajectory_collator.cc

@@ -0,0 +1,61 @@
+/*
+ * 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/sensor/trajectory_collator.h"
+
+namespace cartographer {
+namespace sensor {
+
+void TrajectoryCollator::AddTrajectory(
+    const int trajectory_id,
+    const std::unordered_set<std::string>& expected_sensor_ids,
+    const Callback& callback) {
+  CHECK(trajectory_to_queue_.count(trajectory_id) == 0);
+  for (const auto& sensor_id : expected_sensor_ids) {
+    const auto queue_key = QueueKey{trajectory_id, sensor_id};
+    trajectory_to_queue_[trajectory_id].AddQueue(
+        queue_key, [callback, sensor_id](std::unique_ptr<Data> data) {
+          callback(sensor_id, std::move(data));
+        });
+    trajectory_to_queue_keys_[trajectory_id].push_back(queue_key);
+  }
+}
+
+void TrajectoryCollator::FinishTrajectory(const int trajectory_id) {
+  for (const auto& queue_key : trajectory_to_queue_keys_[trajectory_id]) {
+    trajectory_to_queue_.at(trajectory_id).MarkQueueAsFinished(queue_key);
+  }
+}
+
+void TrajectoryCollator::AddSensorData(const int trajectory_id,
+                                       std::unique_ptr<Data> data) {
+  QueueKey queue_key{trajectory_id, data->GetSensorId()};
+  trajectory_to_queue_.at(trajectory_id)
+      .Add(std::move(queue_key), std::move(data));
+}
+
+void TrajectoryCollator::Flush() {
+  for (auto& it : trajectory_to_queue_) {
+    it.second.Flush();
+  }
+}
+
+common::optional<int> TrajectoryCollator::GetBlockingTrajectoryId() const {
+  return common::optional<int>();
+}
+
+}  // namespace sensor
+}  // namespace cartographer

cartographer/sensor/trajectory_collator.h

@@ -0,0 +1,65 @@
+/*
+ * 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_SENSOR_TRAJECTORY_COLLATOR_H_
+#define CARTOGRAPHER_SENSOR_TRAJECTORY_COLLATOR_H_
+
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "cartographer/sensor/collator_interface.h"
+#include "cartographer/sensor/ordered_multi_queue.h"
+
+namespace cartographer {
+namespace sensor {
+
+// Waits to see at least one data item for all sensor ids and dispatches data
+// in merge-sorted order. Contrary to 'Collator', it does not wait for other
+// trajectories.
+// Also contrary to 'Collator', whose output is deterministic, the sequence in
+// which data is dispatched is not sorted, so non-deterministic input sequences
+// will result in non-deterministic output.
+class TrajectoryCollator : public CollatorInterface {
+ public:
+  TrajectoryCollator() {}
+
+  TrajectoryCollator(const TrajectoryCollator&) = delete;
+  TrajectoryCollator& operator=(const TrajectoryCollator&) = delete;
+
+  void AddTrajectory(int trajectory_id,
+                     const std::unordered_set<std::string>& expected_sensor_ids,
+                     const Callback& callback) override;
+
+  void FinishTrajectory(int trajectory_id) override;
+
+  void AddSensorData(int trajectory_id, std::unique_ptr<Data> data) override;
+
+  void Flush() override;
+
+  common::optional<int> GetBlockingTrajectoryId() const override;
+
+ private:
+  std::unordered_map<int, OrderedMultiQueue> trajectory_to_queue_;
+
+  // Map of trajectory ID to all associated QueueKeys.
+  std::unordered_map<int, std::vector<QueueKey>> trajectory_to_queue_keys_;
+};
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_TRAJECTORY_COLLATOR_H_

cartographer/sensor/trajectory_collator_test.cc

@@ -0,0 +1,121 @@
+/*
+ * 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/sensor/trajectory_collator.h"
+
+#include <array>
+#include <memory>
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
+#include "cartographer/sensor/test_helpers.h"
+#include "cartographer/sensor/timed_point_cloud_data.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace sensor {
+namespace {
+
+using test::CollatorInput;
+using test::CollatorOutput;
+
+TEST(TrajectoryCollator, OrderingMultipleTrajectories) {
+  const int kTrajectoryId[] = {4, 7};
+  const std::array<std::string, 2> kSensorId = {{"my_points", "some_imu"}};
+
+  std::vector<CollatorInput> input_data;
+
+  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
+      kTrajectoryId[0], kSensorId[0], 0));
+  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
+      kTrajectoryId[1], kSensorId[0], 0));
+  input_data.push_back(
+      CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0));
+  input_data.push_back(
+      CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0));
+  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
+      kTrajectoryId[1], kSensorId[0], 100));
+  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
+      kTrajectoryId[0], kSensorId[0], 50));
+  input_data.push_back(
+      CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 60));
+  input_data.push_back(CollatorInput::CreateTimedPointCloudData(
+      kTrajectoryId[1], kSensorId[0], 150));
+  input_data.push_back(
+      CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 120));
+
+  std::vector<CollatorOutput> received;
+  TrajectoryCollator collator;
+  collator.AddTrajectory(
+      kTrajectoryId[0],
+      std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
+      [&received, kTrajectoryId](const std::string& sensor_id,
+                                 std::unique_ptr<Data> data) {
+        received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(),
+                                          data->GetTime()));
+      });
+  collator.AddTrajectory(
+      kTrajectoryId[1],
+      std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
+      [&received, kTrajectoryId](const std::string& sensor_id,
+                                 std::unique_ptr<Data> data) {
+        received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(),
+                                          data->GetTime()));
+      });
+
+  // Send each sensor_id once to establish a common start time.
+  input_data[0].MoveToCollator(&collator);
+  input_data[1].MoveToCollator(&collator);
+  EXPECT_EQ(0, received.size());
+  input_data[2].MoveToCollator(&collator);
+  input_data[3].MoveToCollator(&collator);
+  EXPECT_EQ(2, received.size());
+  EXPECT_EQ(input_data[1].expected_output, received[0]);
+  EXPECT_EQ(input_data[0].expected_output, received[1]);
+
+  // Does not wait for other trajectory.
+  input_data[4].MoveToCollator(&collator);
+  EXPECT_EQ(3, received.size());
+  EXPECT_EQ(input_data[2].expected_output, received[2]);
+
+  input_data[5].MoveToCollator(&collator);
+  EXPECT_EQ(4, received.size());
+  EXPECT_EQ(input_data[3].expected_output, received[3]);
+  input_data[6].MoveToCollator(&collator);
+  EXPECT_EQ(5, received.size());
+  EXPECT_EQ(input_data[5].expected_output, received[4]);
+
+  // Sorts different sensors.
+  input_data[7].MoveToCollator(&collator);
+  EXPECT_EQ(5, received.size());
+  input_data[8].MoveToCollator(&collator);
+  EXPECT_EQ(7, received.size());
+  EXPECT_EQ(input_data[4].expected_output, received[5]);
+  EXPECT_EQ(input_data[8].expected_output, received[6]);
+
+  EXPECT_FALSE(collator.GetBlockingTrajectoryId().has_value());
+
+  collator.FinishTrajectory(kTrajectoryId[0]);
+  collator.FinishTrajectory(kTrajectoryId[1]);
+  collator.Flush();
+  ASSERT_EQ(input_data.size(), received.size());
+}
+
+}  // namespace
+}  // namespace sensor
+}  // namespace cartographer