diff --git a/cartographer/common/CMakeLists.txt b/cartographer/common/CMakeLists.txt
index a608b5f..aa1b978 100644
--- a/cartographer/common/CMakeLists.txt
+++ b/cartographer/common/CMakeLists.txt
@@ -175,6 +175,7 @@ google_test(common_configuration_files_test
   SRCS
     configuration_files_test.cc
   DEPENDS
+    common_config
     common_configuration_file_resolver
     common_lua_parameter_dictionary
     mapping_map_builder
diff --git a/cartographer/common/configuration_files_test.cc b/cartographer/common/configuration_files_test.cc
index 88b6d1a..dec4683 100644
--- a/cartographer/common/configuration_files_test.cc
+++ b/cartographer/common/configuration_files_test.cc
@@ -33,8 +33,9 @@ TEST(ConfigurationFilesTest, ValidateMapBuilderOptions) {
       return MAP_BUILDER)text";
   EXPECT_NO_FATAL_FAILURE({
     auto file_resolver = ::cartographer::common::make_unique<
-        ::cartographer::common::ConfigurationFileResolver>(std::vector<string>{
-        string(::cartographer::common::kSourceDirectory) + "/configuration_files"});
+        ::cartographer::common::ConfigurationFileResolver>(
+        std::vector<string>{string(::cartographer::common::kSourceDirectory) +
+                            "/configuration_files"});
     ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
         kCode, std::move(file_resolver));
     ::cartographer::mapping::CreateMapBuilderOptions(&lua_parameter_dictionary);
diff --git a/cartographer/sensor/ordered_multi_queue.h b/cartographer/sensor/ordered_multi_queue.h
index 0c35f8d..08b4bc3 100644
--- a/cartographer/sensor/ordered_multi_queue.h
+++ b/cartographer/sensor/ordered_multi_queue.h
@@ -168,13 +168,27 @@ class OrderedMultiQueue {
                   << common_start_time_ << "'.";
       }
 
-      if (next_data->time < common_start_time_) {
-        next_queue->queue.Pop();
-        continue;
+      if (next_data->time >= common_start_time_) {
+        // Happy case, we are beyond the 'common_start_time_' already.
+        last_dispatched_time_ = next_data->time;
+        next_queue->callback(next_queue->queue.Pop());
+      } else if (next_queue->queue.Size() < 2) {
+        if (!next_queue->finished) {
+          // We cannot decide whether to drop or dispatch this yet.
+          return;
+        }
+        last_dispatched_time_ = next_data->time;
+        next_queue->callback(next_queue->queue.Pop());
+      } else {
+        // We take a peek at the time after next data. If it also is not beyond
+        // 'common_start_time_' we drop 'next_data', otherwise we just found the
+        // first packet to dispatch from this queue.
+        std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
+        if (next_queue->queue.Peek<Data>()->time > common_start_time_) {
+          last_dispatched_time_ = next_data->time;
+          next_queue->callback(std::move(next_data_owner));
+        }
       }
-
-      last_dispatched_time_ = next_data->time;
-      next_queue->callback(next_queue->queue.Pop());
     }
   }