From 4ef38497a9789a81c75b1380fb81a51b0301941e Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Wed, 6 Jun 2018 14:14:01 +0200
Subject: [PATCH] Skip the Collator for the landmark data. (#1184)

---
 cartographer/mapping/internal/collated_trajectory_builder.cc | 2 ++
 cartographer/mapping/internal/collated_trajectory_builder.h  | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/cartographer/mapping/internal/collated_trajectory_builder.cc b/cartographer/mapping/internal/collated_trajectory_builder.cc
index d4d2e1d..3d607af 100644
--- a/cartographer/mapping/internal/collated_trajectory_builder.cc
+++ b/cartographer/mapping/internal/collated_trajectory_builder.cc
@@ -38,6 +38,8 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
       last_logging_time_(std::chrono::steady_clock::now()) {
   std::unordered_set<std::string> expected_sensor_id_strings;
   for (const auto& sensor_id : expected_sensor_ids) {
+    // Landmark data does not need to be collated.
+    if (sensor_id.type == SensorId::SensorType::LANDMARK) continue;
     expected_sensor_id_strings.insert(sensor_id.id);
   }
   sensor_collator_->AddTrajectory(
diff --git a/cartographer/mapping/internal/collated_trajectory_builder.h b/cartographer/mapping/internal/collated_trajectory_builder.h
index 9bee21f..e454b0e 100644
--- a/cartographer/mapping/internal/collated_trajectory_builder.h
+++ b/cartographer/mapping/internal/collated_trajectory_builder.h
@@ -74,7 +74,7 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
 
   void AddSensorData(const std::string& sensor_id,
                      const sensor::LandmarkData& landmark_data) override {
-    AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
+    wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data);
   }
 
   void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>