diff --git a/cartographer/common/internal/testing/thread_pool_for_testing.cc b/cartographer/common/internal/testing/thread_pool_for_testing.cc
new file mode 100644
index 0000000..eec5e2d
--- /dev/null
+++ b/cartographer/common/internal/testing/thread_pool_for_testing.cc
@@ -0,0 +1,88 @@
+/*
+ * 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/common/internal/testing/thread_pool_for_testing.h"
+
+#include <unistd.h>
+#include <algorithm>
+#include <chrono>
+#include <numeric>
+
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace common {
+namespace testing {
+
+ThreadPoolForTesting::ThreadPoolForTesting()
+    : thread_([this]() { ThreadPoolForTesting::DoWork(); }) {}
+
+ThreadPoolForTesting::~ThreadPoolForTesting() {
+  {
+    MutexLocker locker(&mutex_);
+    CHECK(running_);
+    running_ = false;
+    CHECK_EQ(work_queue_.size(), 0);
+  }
+  thread_.join();
+}
+
+void ThreadPoolForTesting::Schedule(const std::function<void()> &work_item) {
+  MutexLocker locker(&mutex_);
+  idle_ = false;
+  CHECK(running_);
+  work_queue_.push_back(work_item);
+}
+
+void ThreadPoolForTesting::WaitUntilIdle() {
+  for (;;) {
+    {
+      common::MutexLocker locker(&mutex_);
+      if (locker.AwaitWithTimeout([this]() REQUIRES(mutex_) { return idle_; },
+                                  common::FromSeconds(0.1))) {
+        return;
+      }
+    }
+  }
+}
+
+void ThreadPoolForTesting::DoWork() {
+  for (;;) {
+    std::function<void()> work_item;
+    {
+      MutexLocker locker(&mutex_);
+      locker.AwaitWithTimeout(
+          [this]()
+              REQUIRES(mutex_) { return !work_queue_.empty() || !running_; },
+          common::FromSeconds(0.1));
+      if (!work_queue_.empty()) {
+        work_item = work_queue_.front();
+        work_queue_.pop_front();
+      }
+      if (!running_) {
+        return;
+      }
+      if (work_queue_.empty() && !work_item) {
+        idle_ = true;
+      }
+    }
+    if (work_item) work_item();
+  }
+}
+
+}  // namespace testing
+}  // namespace common
+}  // namespace cartographer
diff --git a/cartographer/common/internal/testing/thread_pool_for_testing.h b/cartographer/common/internal/testing/thread_pool_for_testing.h
new file mode 100644
index 0000000..4dc39ff
--- /dev/null
+++ b/cartographer/common/internal/testing/thread_pool_for_testing.h
@@ -0,0 +1,54 @@
+/*
+ * 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_COMMON_INTERNAL_TESTING_THREAD_POOL_FOR_TESTING_H_
+#define CARTOGRAPHER_COMMON_INTERNAL_TESTING_THREAD_POOL_FOR_TESTING_H_
+
+#include <deque>
+#include <functional>
+#include <thread>
+#include <vector>
+
+#include "cartographer/common/mutex.h"
+#include "cartographer/common/thread_pool.h"
+
+namespace cartographer {
+namespace common {
+namespace testing {
+
+class ThreadPoolForTesting : public ThreadPoolInterface {
+ public:
+  ThreadPoolForTesting();
+  ~ThreadPoolForTesting();
+
+  void Schedule(const std::function<void()> &work_item) override;
+  void WaitUntilIdle();
+
+ private:
+  void DoWork();
+
+  std::thread thread_ GUARDED_BY(mutex_);
+  bool running_ GUARDED_BY(mutex_) = true;
+  bool idle_ GUARDED_BY(mutex_) = true;
+  std::deque<std::function<void()>> work_queue_ GUARDED_BY(mutex_);
+  Mutex mutex_;
+};
+
+}  // namespace testing
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_INTERNAL_TESTING_THREAD_POOL_FOR_TESTING_H_
diff --git a/cartographer/common/thread_pool.h b/cartographer/common/thread_pool.h
index ff31321..7ed3508 100644
--- a/cartographer/common/thread_pool.h
+++ b/cartographer/common/thread_pool.h
@@ -27,12 +27,17 @@
 namespace cartographer {
 namespace common {
 
+class ThreadPoolInterface {
+ public:
+  virtual void Schedule(const std::function<void()>& work_item) = 0;
+};
+
 // A fixed number of threads working on a work queue of work items. Adding a
 // new work item does not block, and will be executed by a background thread
 // eventually. The queue must be empty before calling the destructor. The thread
 // pool will then wait for the currently executing work items to finish and then
 // destroy the threads.
-class ThreadPool {
+class ThreadPool : public ThreadPoolInterface {
  public:
   explicit ThreadPool(int num_threads);
   ~ThreadPool();
@@ -40,7 +45,7 @@ class ThreadPool {
   ThreadPool(const ThreadPool&) = delete;
   ThreadPool& operator=(const ThreadPool&) = delete;
 
-  void Schedule(const std::function<void()>& work_item);
+  void Schedule(const std::function<void()>& work_item) override;
 
  private:
   void DoWork();