Initial import of Cartographer codebase.

AUTHORS

@@ -0,0 +1,7 @@
+# This is the list of Cartographer authors for copyright purposes.
+#
+# This does not necessarily list everyone who has contributed code, since in
+# some cases, their employer may be the copyright holder.  To see the full list
+# of contributors, see the revision history in source control.
+Google Inc.
+and other contributors

CONTRIBUTING.md

@@ -0,0 +1,27 @@
+Want to contribute? Great! First, read this page (including the small print at the end).
+
+### Before you contribute
+Before we can use your code, you must sign the
+[Google Individual Contributor License Agreement]
+(https://cla.developers.google.com/about/google-individual)
+(CLA), which you can do online. The CLA is necessary mainly because you own the
+copyright to your changes, even after your contribution becomes part of our
+codebase, so we need your permission to use and distribute your code. We also
+need to be sure of various other things—for instance that you'll tell us if you
+know that your code infringes on other people's patents. You don't have to sign
+the CLA until after you've submitted your code for review and a member has
+approved it, but you must do it before we can put your code into our codebase.
+Before you start working on a larger contribution, you should get in touch with
+us first through the issue tracker with your idea so that we can help out and
+possibly guide you. Coordinating up front makes it much easier to avoid
+frustration later on.
+
+### Code reviews
+All submissions, including submissions by project members, require review. We
+use Github pull requests for this purpose.
+
+### The small print
+Contributions made by corporations are covered by a different agreement than
+the one above, the
+[Software Grant and Corporate Contributor License Agreement]
+(https://cla.developers.google.com/about/google-corporate).

LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   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.

README.md

@@ -0,0 +1,48 @@
+# Cartographer Project Overview
+
+Cartographer is a system that provides real-time simultaneous localization and
+mapping
+([SLAM](http://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping))
+across multiple platforms and sensor configurations.
+
+## Installation instructions
+
+For Ubuntu 14.04 (Trusty):
+
+    sudo apt-get install \
+      g++ \
+      google-mock \
+      libboost-all-dev \
+      libgflags-dev \
+      libgoogle-glog-dev \
+      liblua5.2-dev \
+      libprotobuf-dev \
+      libsuitesparse-dev \
+      libwebp-dev \
+      ninja-build \
+      protobuf-compiler \
+      python-sphinx
+
+Download, build and install Ceres:
+
+    git clone https://ceres-solver.googlesource.com/ceres-solver
+    cd ceres-solver
+    mkdir build && cd build
+    cmake ..
+    make
+    sudo make install
+
+Build Cartographer:
+
+    cd cartographer
+    mkdir build && cd build
+    cmake .. -G Ninja
+    ninja
+
+## Running with Velodyne data
+
+    apt-get install libpcap-dev
+    cd <somwhere>
+    git clone git@github.com:ros-drivers/velodyne.git
+    cd <catkin_ws>/src
+    ln -s <somewhere>/velodyne/velodyne* .

cartographer/CMakeLists.txt

@@ -0,0 +1,134 @@
+# 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.
+
+cmake_minimum_required (VERSION 2.8.7)
+
+project(cartographer)
+set(CARTOGRAPHER_MAJOR_VERSION 1)
+set(CARTOGRAPHER_MINOR_VERSION 0)
+set(CARTOGRAPHER_PATCH_VERSION 0)
+set(CARTOGRAPHER_VERSION ${CARTOGRAPHER_MAJOR_VERSION}.${CARTOGRAPHER_MINOR_VERSION}.${CARTOGRAPHER_PATCH_VERSION})
+set(CARTOGRAPHER_SOVERSION ${CARTOGRAPHER_MAJOR_VERSION}.${CARTOGRAPHER_MINOR_VERSION})
+
+include("${CMAKE_SOURCE_DIR}/cmake/functions.cmake")
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules)
+
+if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "")
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+if(CMAKE_BUILD_TYPE STREQUAL "Release")
+  google_add_flag(GOOG_CXX_FLAGS "-O3 -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo")
+  google_add_flag(GOOG_CXX_FLAGS "-O3 -g -DNDEBUG")
+elseif(CMAKE_BUILD_TYPE STREQUAL "Debug")
+  message(FATAL_ERROR "Cartographer is too slow to be useful in debug mode.")
+else()
+  message(FATAL_ERROR "Unknown CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}")
+endif()
+
+message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
+
+find_package(Boost REQUIRED COMPONENTS system iostreams)
+find_package(Ceres REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(LuaGoogle REQUIRED)
+find_package(Protobuf REQUIRED)
+
+# Only build the documentation if we can find Sphinx.
+find_package(Sphinx)
+if(SPHINX_FOUND)
+  add_subdirectory("doc")
+endif()
+
+set(GMOCK_SRC_DIR "/usr/src/gmock" CACHE STRING "Path to google-mock sources.")
+
+add_subdirectory(${GMOCK_SRC_DIR} "${CMAKE_CURRENT_BINARY_DIR}/gmock")
+
+google_add_flag(GOOG_CXX_FLAGS "-std=c++11")
+
+if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+  google_add_flag(GOOG_CXX_FLAGS "-Weverything")
+  google_add_flag(GOOG_CXX_FLAGS "-Werror=deprecated")
+  google_add_flag(GOOG_CXX_FLAGS "-Werror=non-pod-varargs")
+else()
+  google_add_flag(GOOG_CXX_FLAGS "-Wall")
+  google_add_flag(GOOG_CXX_FLAGS "-Wpedantic")
+endif()
+
+# Turn some warnings into errors. These are defined for clang and gcc.
+google_add_flag(GOOG_CXX_FLAGS "-Werror=format-security")
+google_add_flag(GOOG_CXX_FLAGS "-Werror=return-type")
+google_add_flag(GOOG_CXX_FLAGS "-Werror=uninitialized")
+
+enable_testing()
+
+SET(ALL_LIBRARIES "" CACHE INTERNAL "ALL_LIBRARIES")
+
+# Install catkin package.xml
+install(FILES package.xml DESTINATION share/cartographer)
+
+set(CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY "${CMAKE_INSTALL_PREFIX}/share/cartographer/configuration_files")
+install(DIRECTORY configuration_files DESTINATION share/cartographer/)
+
+add_subdirectory("cartographer")
+
+include(CMakePackageConfigHelpers)
+
+# Create a cartographer-config.cmake file for the use from the install tree
+# and install it
+set(CARTOGRAPHER_LIBRARY_DIRS "${CMAKE_INSTALL_PREFIX}/lib")
+list(APPEND CARTOGRAPHER_LIBRARY_DIRS "${LUA_LIBRARY_DIR}")
+
+set(CARTOGRAPHER_INCLUDE_DIRS "${CMAKE_INSTALL_PREFIX}/include")
+list(APPEND CARTOGRAPHER_INCLUDE_DIRS "${LUA_INCLUDE_DIR}")
+list(APPEND CARTOGRAPHER_INCLUDE_DIRS "${CERES_INCLUDE_DIRS}")
+list(APPEND CARTOGRAPHER_INCLUDE_DIRS "${PROTOBUF_INCLUDE_DIR}")
+
+google_combined_library(cartographer
+  SRCS "${ALL_LIBRARIES}"
+)
+
+get_property(CARTOGRAPHER_LIBRARY_FILE TARGET cartographer PROPERTY LOCATION)
+install(
+  FILES
+    ${CARTOGRAPHER_LIBRARY_FILE}
+  DESTINATION
+    lib
+)
+
+# TODO(hrapp): We have to mention glog here, otherwise we get unresolved
+# symbols. Piggybacking on Ceres does not seem to do the trick.
+set(CARTOGRAPHER_LIBRARIES "")
+list(APPEND CARTOGRAPHER_LIBRARIES "cartographer")
+list(APPEND CARTOGRAPHER_LIBRARIES "${CERES_LIBRARIES}")
+list(APPEND CARTOGRAPHER_LIBRARIES "${Boost_LIBRARIES}")
+list(APPEND CARTOGRAPHER_LIBRARIES "${LUA_LIBRARIES}")
+list(APPEND CARTOGRAPHER_LIBRARIES "${PROTOBUF_LIBRARIES}")
+list(APPEND CARTOGRAPHER_LIBRARIES "webp")
+list(APPEND CARTOGRAPHER_LIBRARIES "glog")
+
+CONFIGURE_PACKAGE_CONFIG_FILE(
+  cartographer-config.cmake.in
+  "${CMAKE_BINARY_DIR}/cmake/cartographer/cartographer-config.cmake"
+  PATH_VARS CARTOGRAPHER_INCLUDE_DIRS CARTOGRAPHER_LIBRARY_DIRS
+  INSTALL_DESTINATION ${CMAKE_INSTALL_PREFIX}/share/cartographer
+)
+
+install(
+  FILES
+    "${CMAKE_BINARY_DIR}/cmake/cartographer/cartographer-config.cmake"
+  DESTINATION
+    share/cartographer/
+)

cartographer/cartographer-config.cmake.in

@@ -0,0 +1,45 @@
+# 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.
+
+#  Usage from an external project:
+#    In your CMakeLists.txt, add these lines:
+#
+#    find_package(Cartographer REQUIRED )
+#    include_directories(${CARTOGRAPHER_INCLUDE_DIRS})
+#    target_link_libraries(MY_TARGET_NAME ${CARTOGRAPHER_LIBRARIES})
+#
+#
+#    This file will define the following variables:
+#      - CARTOGRAPHER_LIBRARIES
+#      - CARTOGRAPHER_LIBRARY_DIRS
+#      - CARTOGRAPHER_INCLUDE_DIRS
+
+@PACKAGE_INIT@
+
+set(CARTOGRAPHER_INCLUDE_DIRS "@PACKAGE_CARTOGRAPHER_INCLUDE_DIRS@")
+set(CARTOGRAPHER_LIBRARY_DIRS "@PACKAGE_CARTOGRAPHER_LIBRARY_DIRS@")
+
+set(CARTOGRAPHER_LIBRARIES
+  "@CARTOGRAPHER_LIBRARIES@"
+)
+
+set(CERES_DIR_HINTS @Ceres_DIR@)
+
+if (cartographer_FIND_QUIETLY)
+   find_package(Ceres QUIET HINTS ${CERES_DIR_HINTS})
+elseif (cartographer_FIND_REQUIRED)
+   find_package(Ceres REQUIRED HINTS ${CERES_DIR_HINTS})
+else ()
+   find_package(Ceres HINTS ${CERES_DIR_HINTS})
+endif()

cartographer/cartographer/CMakeLists.txt

@@ -0,0 +1,22 @@
+# 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.
+
+add_subdirectory("common")
+add_subdirectory("kalman_filter")
+add_subdirectory("mapping")
+add_subdirectory("mapping_2d")
+add_subdirectory("mapping_3d")
+add_subdirectory("proto")
+add_subdirectory("sensor")
+add_subdirectory("transform")

cartographer/cartographer/common/CMakeLists.txt

@@ -0,0 +1,198 @@
+# 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.
+
+add_subdirectory("proto")
+
+configure_file (
+  ${CMAKE_CURRENT_SOURCE_DIR}/config.h.cmake
+  ${CMAKE_CURRENT_BINARY_DIR}/config.h)
+
+google_library(common_blocking_queue
+  USES_CERES
+  HDRS
+    blocking_queue.h
+  DEPENDS
+    common_mutex
+    common_port
+    common_time
+)
+
+google_library(common_ceres_solver_options
+  USES_CERES
+  SRCS
+    ceres_solver_options.cc
+  HDRS
+    ceres_solver_options.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_proto_ceres_solver_options
+)
+
+google_library(common_config
+  HDRS
+    config.h
+)
+
+google_library(common_configuration_file_resolver
+  USES_CERES
+  SRCS
+    configuration_file_resolver.cc
+  HDRS
+    configuration_file_resolver.h
+  DEPENDS
+    common_config
+    common_lua_parameter_dictionary
+    common_port
+)
+
+google_library(common_fixed_ratio_sampler
+  SRCS
+    fixed_ratio_sampler.cc
+  HDRS
+    fixed_ratio_sampler.h
+  DEPENDS
+    common_port
+)
+
+google_library(common_histogram
+  USES_CERES
+  SRCS
+    histogram.cc
+  HDRS
+    histogram.h
+  DEPENDS
+    common_port
+)
+
+google_library(common_lua
+  USES_LUA
+  HDRS
+    lua.h
+)
+
+google_library(common_lua_parameter_dictionary
+  USES_CERES
+  SRCS
+    lua_parameter_dictionary.cc
+  HDRS
+    lua_parameter_dictionary.h
+  DEPENDS
+    common_lua
+    common_port
+)
+
+google_library(common_lua_parameter_dictionary_test_helpers
+  USES_CERES
+  HDRS
+    lua_parameter_dictionary_test_helpers.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_make_unique
+    common_port
+)
+
+google_library(common_make_unique
+  HDRS
+    make_unique.h
+)
+
+google_library(common_math
+  USES_CERES
+  USES_EIGEN
+  HDRS
+    math.h
+  DEPENDS
+    common_port
+)
+
+google_library(common_mutex
+  HDRS
+    mutex.h
+  DEPENDS
+    common_time
+)
+
+google_library(common_ordered_multi_queue
+  HDRS
+    ordered_multi_queue.h
+  DEPENDS
+    common_blocking_queue
+    common_make_unique
+    common_port
+)
+
+google_library(common_port
+  USES_BOOST
+  HDRS
+    port.h
+)
+
+google_library(common_thread_pool
+  USES_CERES
+  SRCS
+    thread_pool.cc
+  HDRS
+    thread_pool.h
+  DEPENDS
+    common_mutex
+)
+
+google_library(common_time
+  SRCS
+    time.cc
+  HDRS
+    time.h
+  DEPENDS
+    common_port
+)
+
+google_test(common_blocking_queue_test
+  SRCS
+    blocking_queue_test.cc
+  DEPENDS
+    common_blocking_queue
+    common_make_unique
+    common_time
+)
+
+google_test(common_fixed_ratio_sampler_test
+  SRCS
+    fixed_ratio_sampler_test.cc
+  DEPENDS
+    common_fixed_ratio_sampler
+)
+
+google_test(common_lua_parameter_dictionary_test
+  SRCS
+    lua_parameter_dictionary_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_lua_parameter_dictionary_test_helpers
+    common_make_unique
+)
+
+google_test(common_math_test
+  SRCS
+    math_test.cc
+  DEPENDS
+    common_math
+)
+
+google_test(common_ordered_multi_queue_test
+  SRCS
+    ordered_multi_queue_test.cc
+  DEPENDS
+    common_make_unique
+    common_ordered_multi_queue
+)

cartographer/cartographer/common/blocking_queue.h

@@ -0,0 +1,125 @@
+/*
+ * 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_COMMON_BLOCKING_QUEUE_H_
+#define CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_
+
+#include <cstddef>
+#include <deque>
+#include <memory>
+
+#include "cartographer/common/mutex.h"
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace common {
+
+// A thread-safe blocking queue that is useful for producer/consumer patterns.
+// 'T' must be movable.
+template <typename T>
+class BlockingQueue {
+ public:
+  static constexpr size_t kInfiniteQueueSize = 0;
+
+  // Constructs a blocking queue with infinite queue size.
+  BlockingQueue() : BlockingQueue(kInfiniteQueueSize) {}
+
+  BlockingQueue(const BlockingQueue&) = delete;
+  BlockingQueue& operator=(const BlockingQueue&) = delete;
+
+  // Constructs a blocking queue with a size of 'queue_size'.
+  explicit BlockingQueue(const size_t queue_size) : queue_size_(queue_size) {}
+
+  // Pushes a value onto the queue. Blocks if the queue is full.
+  void Push(T t) {
+    MutexLocker lock(&mutex_);
+    lock.Await([this]() REQUIRES(mutex_) { return QueueNotFullCondition(); });
+    deque_.push_back(std::move(t));
+  }
+
+  // Like push, but returns false if 'timeout' is reached.
+  bool PushWithTimeout(T t, const common::Duration timeout) {
+    MutexLocker lock(&mutex_);
+    if (!lock.AwaitWithTimeout(
+            [this]() REQUIRES(mutex_) { return QueueNotFullCondition(); },
+            timeout)) {
+      return false;
+    }
+    deque_.push_back(std::move(t));
+    return true;
+  }
+
+  // Pops the next value from the queue. Blocks until a value is available.
+  T Pop() {
+    MutexLocker lock(&mutex_);
+    lock.Await([this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); });
+
+    T t = std::move(deque_.front());
+    deque_.pop_front();
+    return t;
+  }
+
+  // Like Pop, but can timeout. Returns nullptr in this case.
+  T PopWithTimeout(const common::Duration timeout) {
+    MutexLocker lock(&mutex_);
+    if (!lock.AwaitWithTimeout(
+            [this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); },
+            timeout)) {
+      return nullptr;
+    }
+    T t = std::move(deque_.front());
+    deque_.pop_front();
+    return t;
+  }
+
+  // Returns the next value in the queue or nullptr if the queue is empty.
+  // Maintains ownership. This assumes a member function get() that returns
+  // a pointer to the given type R.
+  template <typename R>
+  const R* Peek() {
+    MutexLocker lock(&mutex_);
+    if (deque_.empty()) {
+      return nullptr;
+    }
+    return deque_.front().get();
+  }
+
+  // Returns the number of items currently in the queue.
+  size_t Size() {
+    MutexLocker lock(&mutex_);
+    return deque_.size();
+  }
+
+ private:
+  // Returns true iff the queue is not empty.
+  bool QueueNotEmptyCondition() REQUIRES(mutex_) { return !deque_.empty(); }
+
+  // Returns true iff the queue is not full.
+  bool QueueNotFullCondition() REQUIRES(mutex_) {
+    return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
+  }
+
+  Mutex mutex_;
+  const size_t queue_size_ GUARDED_BY(mutex_);
+  std::deque<T> deque_ GUARDED_BY(mutex_);
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_

cartographer/cartographer/common/blocking_queue_test.cc

@@ -0,0 +1,119 @@
+/*
+ * 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/common/blocking_queue.h"
+
+#include <memory>
+#include <thread>
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/time.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace common {
+namespace {
+
+TEST(BlockingQueueTest, testPushPeekPop) {
+  BlockingQueue<std::unique_ptr<int>> blocking_queue;
+  blocking_queue.Push(common::make_unique<int>(42));
+  ASSERT_EQ(1, blocking_queue.Size());
+  blocking_queue.Push(common::make_unique<int>(24));
+  ASSERT_EQ(2, blocking_queue.Size());
+  EXPECT_EQ(42, *blocking_queue.Peek<int>());
+  ASSERT_EQ(2, blocking_queue.Size());
+  EXPECT_EQ(42, *blocking_queue.Pop());
+  ASSERT_EQ(1, blocking_queue.Size());
+  EXPECT_EQ(24, *blocking_queue.Pop());
+  ASSERT_EQ(0, blocking_queue.Size());
+  EXPECT_EQ(nullptr, blocking_queue.Peek<int>());
+  ASSERT_EQ(0, blocking_queue.Size());
+}
+
+TEST(BlockingQueueTest, testPushPopSharedPtr) {
+  BlockingQueue<std::shared_ptr<int>> blocking_queue;
+  blocking_queue.Push(std::make_shared<int>(42));
+  blocking_queue.Push(std::make_shared<int>(24));
+  EXPECT_EQ(42, *blocking_queue.Pop());
+  EXPECT_EQ(24, *blocking_queue.Pop());
+}
+
+TEST(BlockingQueueTest, testPopWithTimeout) {
+  BlockingQueue<std::unique_ptr<int>> blocking_queue;
+  EXPECT_EQ(nullptr,
+            blocking_queue.PopWithTimeout(common::FromMilliseconds(150)));
+}
+
+TEST(BlockingQueueTest, testPushWithTimeout) {
+  BlockingQueue<std::unique_ptr<int>> blocking_queue(1);
+  EXPECT_EQ(true,
+            blocking_queue.PushWithTimeout(common::make_unique<int>(42),
+                                           common::FromMilliseconds(150)));
+  EXPECT_EQ(false,
+            blocking_queue.PushWithTimeout(common::make_unique<int>(15),
+                                           common::FromMilliseconds(150)));
+  EXPECT_EQ(42, *blocking_queue.Pop());
+  EXPECT_EQ(0, blocking_queue.Size());
+}
+
+TEST(BlockingQueueTest, testPushWithTimeoutInfinteQueue) {
+  BlockingQueue<std::unique_ptr<int>> blocking_queue;
+  EXPECT_EQ(true,
+            blocking_queue.PushWithTimeout(common::make_unique<int>(42),
+                                           common::FromMilliseconds(150)));
+  EXPECT_EQ(true,
+            blocking_queue.PushWithTimeout(common::make_unique<int>(45),
+                                           common::FromMilliseconds(150)));
+  EXPECT_EQ(42, *blocking_queue.Pop());
+  EXPECT_EQ(45, *blocking_queue.Pop());
+  EXPECT_EQ(0, blocking_queue.Size());
+}
+
+TEST(BlockingQueueTest, testBlockingPop) {
+  BlockingQueue<std::unique_ptr<int>> blocking_queue;
+  ASSERT_EQ(0, blocking_queue.Size());
+
+  int pop = 0;
+
+  std::thread thread([&blocking_queue, &pop] { pop = *blocking_queue.Pop(); });
+
+  std::this_thread::sleep_for(common::FromMilliseconds(100));
+  blocking_queue.Push(common::make_unique<int>(42));
+  thread.join();
+  ASSERT_EQ(0, blocking_queue.Size());
+  EXPECT_EQ(42, pop);
+}
+
+TEST(BlockingQueueTest, testBlockingPopWithTimeout) {
+  BlockingQueue<std::unique_ptr<int>> blocking_queue;
+  ASSERT_EQ(0, blocking_queue.Size());
+
+  int pop = 0;
+
+  std::thread thread([&blocking_queue, &pop] {
+    pop = *blocking_queue.PopWithTimeout(common::FromMilliseconds(2500));
+  });
+
+  std::this_thread::sleep_for(common::FromMilliseconds(100));
+  blocking_queue.Push(common::make_unique<int>(42));
+  thread.join();
+  ASSERT_EQ(0, blocking_queue.Size());
+  EXPECT_EQ(42, pop);
+}
+
+}  // namespace
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/ceres_solver_options.cc

@@ -0,0 +1,45 @@
+/*
+ * 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/common/ceres_solver_options.h"
+
+namespace cartographer {
+namespace common {
+
+proto::CeresSolverOptions CreateCeresSolverOptionsProto(
+    common::LuaParameterDictionary* parameter_dictionary) {
+  proto::CeresSolverOptions proto;
+  proto.set_use_nonmonotonic_steps(
+      parameter_dictionary->GetBool("use_nonmonotonic_steps"));
+  proto.set_max_num_iterations(
+      parameter_dictionary->GetNonNegativeInt("max_num_iterations"));
+  proto.set_num_threads(parameter_dictionary->GetNonNegativeInt("num_threads"));
+  CHECK_GT(proto.max_num_iterations(), 0);
+  CHECK_GT(proto.num_threads(), 0);
+  return proto;
+}
+
+ceres::Solver::Options CreateCeresSolverOptions(
+    const proto::CeresSolverOptions& proto) {
+  ceres::Solver::Options options;
+  options.use_nonmonotonic_steps = proto.use_nonmonotonic_steps();
+  options.max_num_iterations = proto.max_num_iterations();
+  options.num_threads = proto.num_threads();
+  return options;
+}
+
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/ceres_solver_options.h

@@ -0,0 +1,36 @@
+/*
+ * 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_COMMON_CERES_SOLVER_OPTIONS_H_
+#define CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/proto/ceres_solver_options.pb.h"
+#include "ceres/ceres.h"
+
+namespace cartographer {
+namespace common {
+
+proto::CeresSolverOptions CreateCeresSolverOptionsProto(
+    common::LuaParameterDictionary* parameter_dictionary);
+
+ceres::Solver::Options CreateCeresSolverOptions(
+    const proto::CeresSolverOptions& proto);
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_

cartographer/cartographer/common/config.h.cmake

@@ -0,0 +1,29 @@
+/*
+ * 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_COMMON_CONFIG_H_
+#define CARTOGRAPHER_COMMON_CONFIG_H_
+
+namespace cartographer {
+namespace common {
+
+constexpr char kConfigurationFilesDirectory[] =
+    "@CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY@";
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_CONFIG_H_

cartographer/cartographer/common/configuration_file_resolver.cc

@@ -0,0 +1,54 @@
+/*
+ * 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/common/configuration_file_resolver.h"
+
+#include <fstream>
+#include <iostream>
+#include <streambuf>
+
+#include "cartographer/common/config.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace common {
+
+ConfigurationFileResolver::ConfigurationFileResolver(
+    const std::vector<string>& configuration_files_directories)
+    : configuration_files_directories_(configuration_files_directories) {
+  configuration_files_directories_.push_back(kConfigurationFilesDirectory);
+}
+
+string ConfigurationFileResolver::GetFullPathOrDie(const string& basename) {
+  for (const auto& path : configuration_files_directories_) {
+    const string filename = path + "/" + basename;
+    std::ifstream stream(filename.c_str());
+    if (stream.good()) {
+      return filename;
+    }
+  }
+  LOG(FATAL) << "File " << basename << " was not found.";
+}
+
+string ConfigurationFileResolver::GetFileContentOrDie(const string& basename) {
+  const string filename = GetFullPathOrDie(basename);
+  std::ifstream stream(filename.c_str());
+  return string((std::istreambuf_iterator<char>(stream)),
+                std::istreambuf_iterator<char>());
+}
+
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/configuration_file_resolver.h

@@ -0,0 +1,49 @@
+/*
+ * 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_COMMON_CONFIGURATION_FILE_RESOLVER_H_
+#define CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_
+
+#include <vector>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+
+namespace cartographer {
+namespace common {
+
+// A 'FileResolver' for the 'LuaParameterDictionary' that reads files from disk.
+// It searches the 'configuration_files_directories' in order to find the
+// requested filename. The last place searched is always the
+// 'configuration_files/' directory installed with Cartographer. It contains
+// reasonable configuration for the various Cartographer components which
+// provide a good starting ground for new platforms.
+class ConfigurationFileResolver : public FileResolver {
+ public:
+  explicit ConfigurationFileResolver(
+      const std::vector<string>& configuration_files_directories);
+
+  string GetFullPathOrDie(const string& basename) override;
+  string GetFileContentOrDie(const string& basename) override;
+
+ private:
+  std::vector<string> configuration_files_directories_;
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_

cartographer/cartographer/common/fixed_ratio_sampler.cc

@@ -0,0 +1,41 @@
+/*
+ * 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/common/fixed_ratio_sampler.h"
+
+namespace cartographer {
+namespace common {
+
+FixedRatioSampler::FixedRatioSampler(const double ratio) : ratio_(ratio) {}
+
+FixedRatioSampler::~FixedRatioSampler() {}
+
+bool FixedRatioSampler::Pulse() {
+  ++num_pulses_;
+  if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
+    ++num_samples_;
+    return true;
+  }
+  return false;
+}
+
+string FixedRatioSampler::DebugString() {
+  return std::to_string(num_samples_) + " (" +
+         std::to_string(100. * num_samples_ / num_pulses_) + "%)";
+}
+
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/fixed_ratio_sampler.h

@@ -0,0 +1,55 @@
+/*
+ * 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_COMMON_FIXED_RATIO_SAMPLER_H_
+#define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_
+
+#include <string>
+
+#include "cartographer/common/port.h"
+
+namespace cartographer {
+namespace common {
+
+// Signals when a sample should be taken from a stream of data to select a
+// uniformly distributed fraction of the data.
+class FixedRatioSampler {
+ public:
+  explicit FixedRatioSampler(double ratio);
+  ~FixedRatioSampler();
+
+  FixedRatioSampler(const FixedRatioSampler&) = delete;
+  FixedRatioSampler& operator=(const FixedRatioSampler&) = delete;
+
+  // Returns true if this pulse should result in an sample.
+  bool Pulse();
+
+  // Returns a debug string describing the current ratio of samples to pulses.
+  string DebugString();
+
+ private:
+  // Sampling occurs if the proportion of samples to pulses drops below this
+  // number.
+  const double ratio_;
+
+  int64 num_pulses_ = 0;
+  int64 num_samples_ = 0;
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_

cartographer/cartographer/common/fixed_ratio_sampler_test.cc

@@ -0,0 +1,57 @@
+/*
+ * 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/common/fixed_ratio_sampler.h"
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace common {
+namespace {
+
+TEST(FixedRatioSamplerTest, AlwaysTrue) {
+  FixedRatioSampler fixed_ratio_sampler(1.);
+  for (int i = 0; i < 100; ++i) {
+    EXPECT_TRUE(fixed_ratio_sampler.Pulse());
+  }
+}
+
+TEST(FixedRatioSamplerTest, AlwaysFalse) {
+  FixedRatioSampler fixed_ratio_sampler(0.);
+  for (int i = 0; i < 100; ++i) {
+    EXPECT_FALSE(fixed_ratio_sampler.Pulse());
+  }
+}
+
+TEST(FixedRatioSamplerTest, SometimesTrue) {
+  FixedRatioSampler fixed_ratio_sampler(0.5);
+  for (int i = 0; i < 100; ++i) {
+    EXPECT_EQ(i % 2 == 0, fixed_ratio_sampler.Pulse());
+  }
+}
+
+TEST(FixedRatioSamplerTest, FirstPulseIsTrue) {
+  // Choose a very very small positive number for the ratio.
+  FixedRatioSampler fixed_ratio_sampler(1e-20);
+  EXPECT_TRUE(fixed_ratio_sampler.Pulse());
+  for (int i = 0; i < 100; ++i) {
+    EXPECT_FALSE(fixed_ratio_sampler.Pulse());
+  }
+}
+
+}  // namespace
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/histogram.cc

@@ -0,0 +1,113 @@
+/*
+ * 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/common/histogram.h"
+
+#include <algorithm>
+#include <numeric>
+#include <string>
+
+#include "cartographer/common/port.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace common {
+
+namespace {
+
+string PaddedTo(string input, int new_length) {
+  CHECK_GE(new_length, input.size());
+  input.insert(input.begin(), new_length - input.size(), ' ');
+  return input;
+}
+
+}  // namespace
+
+void BucketHistogram::Hit(const string& bucket) { ++buckets_[bucket]; }
+
+string BucketHistogram::ToString() const {
+  int64 sum = 0;
+  size_t max_bucket_name_length = 0;
+  for (const auto& pair : buckets_) {
+    sum += pair.second;
+    max_bucket_name_length =
+        std::max(pair.first.size(), max_bucket_name_length);
+  }
+
+  string result;
+  for (const auto& pair : buckets_) {
+    const float percent = 100.f * pair.second / std::max<int64>(1, sum);
+    result += PaddedTo(pair.first, max_bucket_name_length) + ": " +
+              PaddedTo(std::to_string(pair.second), 7) + " (" +
+              std::to_string(percent) + " %)\n";
+  }
+  result += "Total: " + std::to_string(sum);
+  return result;
+}
+
+void Histogram::Add(const float value) { values_.push_back(value); }
+
+string Histogram::ToString(const int buckets) const {
+  CHECK_GE(buckets, 1);
+  if (values_.empty()) {
+    return "Count: 0";
+  }
+  const float min = *std::min_element(values_.begin(), values_.end());
+  const float max = *std::max_element(values_.begin(), values_.end());
+  const float mean =
+      std::accumulate(values_.begin(), values_.end(), 0.f) / values_.size();
+  string result = "Count: " + std::to_string(values_.size()) + "  Min: " +
+                  std::to_string(min) + "  Max: " + std::to_string(max) +
+                  "  Mean: " + std::to_string(mean);
+  if (min == max) {
+    return result;
+  }
+  CHECK_LT(min, max);
+  float lower_bound = min;
+  int total_count = 0;
+  for (int i = 0; i != buckets; ++i) {
+    const float upper_bound =
+        (i + 1 == buckets)
+            ? max
+            : (max * (i + 1) / buckets + min * (buckets - i - 1) / buckets);
+    int count = 0;
+    for (const float value : values_) {
+      if (lower_bound <= value &&
+          (i + 1 == buckets ? value <= upper_bound : value < upper_bound)) {
+        ++count;
+      }
+    }
+    total_count += count;
+    result += "\n[" + std::to_string(lower_bound) + ", " +
+              std::to_string(upper_bound) + ((i + 1 == buckets) ? "]" : ")");
+    constexpr int kMaxBarChars = 20;
+    const int bar =
+        (count * kMaxBarChars + values_.size() / 2) / values_.size();
+    result += "\t";
+    for (int i = 0; i != kMaxBarChars; ++i) {
+      result += (i < (kMaxBarChars - bar)) ? " " : "#";
+    }
+    result += "\tCount: " + std::to_string(count) + " (" +
+              std::to_string(count * 1e2f / values_.size()) + "%)";
+    result += "\tTotal: " + std::to_string(total_count) + " (" +
+              std::to_string(total_count * 1e2f / values_.size()) + "%)";
+    lower_bound = upper_bound;
+  }
+  return result;
+}
+
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/histogram.h

@@ -0,0 +1,50 @@
+/*
+ * 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_COMMON_HISTOGRAM_H_
+#define CARTOGRAPHER_COMMON_HISTOGRAM_H_
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "cartographer/common/port.h"
+
+namespace cartographer {
+namespace common {
+
+class BucketHistogram {
+ public:
+  void Hit(const string& bucket);
+  string ToString() const;
+
+ private:
+  std::map<string, int64> buckets_;
+};
+
+class Histogram {
+ public:
+  void Add(float value);
+  string ToString(int buckets) const;
+
+ private:
+  std::vector<float> values_;
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_HISTOGRAM_H_

cartographer/cartographer/common/lua.h

@@ -0,0 +1,22 @@
+/*
+ * 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_COMMON_LUA_H_
+#define CARTOGRAPHER_COMMON_LUA_H_
+
+#include <lua.hpp>
+
+#endif  // CARTOGRAPHER_COMMON_LUA_H_

cartographer/cartographer/common/lua_parameter_dictionary.cc

@@ -0,0 +1,498 @@
+/*
+ * 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.
+ */
+
+// When a LuaParameterDictionary is constructed, a new Lua state (i.e. an
+// independent Lua interpreter) is fired up to evaluate the Lua code. The code
+// is expected to return a Lua table that contains key/value pairs that are the
+// key/value pairs of our parameter dictionary.
+//
+// We keep the Lua interpreter around and the table on the stack and reference
+// it in the Get* methods instead of moving its contents from Lua into a C++ map
+// since we can only know in the Get* methods what data type to expect in the
+// table.
+//
+// Some of the methods below documentation the current stack with the following
+// notation: S: <bottom> ... <top>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <memory>
+
+namespace cartographer {
+namespace common {
+
+namespace {
+
+// Replace the string at the top of the stack through a quoted version that Lua
+// can read back.
+void QuoteStringOnStack(lua_State* L) {
+  CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value.";
+  int current_index = lua_gettop(L);
+
+  // S: ... string
+  lua_pushglobaltable(L);         // S: ... string globals
+  lua_getfield(L, -1, "string");  // S: ... string globals <string module>
+  lua_getfield(L, -1,
+               "format");   // S: ... string globals <string module> format
+  lua_pushstring(L, "%q");  // S: ... string globals <string module> format "%q"
+  lua_pushvalue(L, current_index);  // S: ... string globals <string module>
+                                    // format "%q" string
+
+  lua_call(L, 2, 1);  // S: ... string globals <string module> quoted
+  lua_replace(L, current_index);  // S: ... quoted globals <string module>
+
+  lua_pop(L, 2);  // S: ... quoted
+}
+
+// Sets the given 'dictionary' as the value of the "this" key in Lua's registry
+// table.
+void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) {
+  lua_pushstring(L, "this");
+  lua_pushlightuserdata(L, dictionary);
+  lua_settable(L, LUA_REGISTRYINDEX);
+}
+
+// Gets the 'dictionary' from the "this" key in Lua's registry table.
+LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) {
+  lua_pushstring(L, "this");
+  lua_gettable(L, LUA_REGISTRYINDEX);
+  void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1);
+  lua_pop(L, 1);
+  CHECK(return_value != nullptr);
+  return reinterpret_cast<LuaParameterDictionary*>(return_value);
+}
+
+// CHECK()s if a Lua method returned an error.
+void CheckForLuaErrors(lua_State* L, int status) {
+  CHECK_EQ(status, 0) << lua_tostring(L, -1);
+}
+
+// Returns 'a' if 'condition' is true, else 'b'.
+int LuaChoose(lua_State* L) {
+  CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b).";
+  CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value.";
+
+  const bool condition = lua_toboolean(L, 1);
+  if (condition) {
+    lua_pushvalue(L, 2);
+  } else {
+    lua_pushvalue(L, 3);
+  }
+  return 1;
+}
+
+// Pushes a value to the Lua stack.
+void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); }
+void PushValue(lua_State* L, const string& key) {
+  lua_pushstring(L, key.c_str());
+}
+
+// Reads the value with the given 'key' from the Lua dictionary and pushes it to
+// the top of the stack.
+template <typename T>
+void GetValueFromLuaTable(lua_State* L, const T& key) {
+  PushValue(L, key);
+  lua_rawget(L, -2);
+}
+
+// CHECK() that the topmost parameter on the Lua stack is a table.
+void CheckTableIsAtTopOfStack(lua_State* L) {
+  CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
+}
+
+// Returns true if 'key' is in the table at the top of the Lua stack.
+template <typename T>
+bool HasKeyOfType(lua_State* L, const T& key) {
+  CheckTableIsAtTopOfStack(L);
+  PushValue(L, key);
+  lua_rawget(L, -2);
+  const bool key_not_found = lua_isnil(L, -1);
+  lua_pop(L, 1);  // Pop the item again.
+  return !key_not_found;
+}
+
+// Iterates over the integer keys of the table at the top of the stack of 'L•
+// and pushes the values one by one. 'pop_value' is expected to pop a value and
+// put them into a C++ container.
+void GetArrayValues(lua_State* L, const std::function<void()>& pop_value) {
+  int idx = 1;
+  while (true) {
+    GetValueFromLuaTable(L, idx);
+    if (lua_isnil(L, -1)) {
+      lua_pop(L, 1);
+      break;
+    }
+    pop_value();
+    ++idx;
+  }
+}
+
+}  // namespace
+
+std::unique_ptr<LuaParameterDictionary>
+LuaParameterDictionary::NonReferenceCounted(
+    const string& code, std::unique_ptr<FileResolver> file_resolver,
+    StateExtensionFunction state_extension_function) {
+  return std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
+      code, ReferenceCount::NO, std::move(file_resolver),
+      state_extension_function));
+}
+
+std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::Partial(
+    const string& code, const string& key,
+    std::unique_ptr<FileResolver> file_resolver,
+    StateExtensionFunction state_extension_function) {
+  auto parameter_dictionary =
+      std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
+          code, std::move(file_resolver), state_extension_function));
+  // This replaces the table at the top of the stack with the table at 'key'.
+  auto& L = parameter_dictionary->L_;
+
+  const string lua_code = "local table=...; return table." + key;
+  CheckForLuaErrors(L, luaL_loadstring(L, lua_code.c_str()));
+  lua_pushvalue(L, -2);  // S: table, function, table
+  lua_remove(L, -3);     // S: function, table
+  CheckForLuaErrors(L, lua_pcall(L, 1, 1, 0));
+  CheckTableIsAtTopOfStack(L);
+  return parameter_dictionary;
+}
+
+LuaParameterDictionary::LuaParameterDictionary(
+    const string& code, std::unique_ptr<FileResolver> file_resolver,
+    StateExtensionFunction state_extension_function)
+    : LuaParameterDictionary(code, ReferenceCount::YES,
+                             std::move(file_resolver),
+                             state_extension_function) {}
+
+LuaParameterDictionary::LuaParameterDictionary(
+    const string& code, ReferenceCount reference_count,
+    std::unique_ptr<FileResolver> file_resolver,
+    StateExtensionFunction state_extension_function)
+    : L_(luaL_newstate()),
+      index_into_reference_table_(-1),
+      file_resolver_(std::move(file_resolver)),
+      reference_count_(reference_count) {
+  CHECK_NOTNULL(L_);
+  SetDictionaryInRegistry(L_, this);
+
+  luaL_openlibs(L_);
+
+  lua_register(L_, "choose", LuaChoose);
+  lua_register(L_, "include", LuaInclude);
+  lua_register(L_, "read", LuaRead);
+  if (state_extension_function) {
+    state_extension_function(L_);
+  }
+
+  CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str()));
+  CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0));
+  CheckTableIsAtTopOfStack(L_);
+}
+
+LuaParameterDictionary::LuaParameterDictionary(
+    lua_State* const L, ReferenceCount reference_count,
+    std::shared_ptr<FileResolver> file_resolver)
+    : L_(lua_newthread(L)),
+      file_resolver_(std::move(file_resolver)),
+      reference_count_(reference_count) {
+  CHECK_NOTNULL(L_);
+
+  // Make sure this is never garbage collected.
+  CHECK(lua_isthread(L, -1));
+  index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX);
+
+  CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
+  lua_xmove(L, L_, 1);  // Moves the table and the coroutine over.
+  CheckTableIsAtTopOfStack(L_);
+}
+
+LuaParameterDictionary::~LuaParameterDictionary() {
+  if (reference_count_ == ReferenceCount::YES) {
+    CheckAllKeysWereUsedExactlyOnceAndReset();
+  }
+  if (index_into_reference_table_ > 0) {
+    luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_);
+  } else {
+    lua_close(L_);
+  }
+}
+
+std::vector<string> LuaParameterDictionary::GetKeys() const {
+  CheckTableIsAtTopOfStack(L_);
+  std::vector<string> keys;
+
+  lua_pushnil(L_);  // Push the first key
+  while (lua_next(L_, -2) != 0) {
+    lua_pop(L_, 1);  // Pop value, keep key.
+    if (!lua_isnumber(L_, -1)) {
+      keys.emplace_back(lua_tostring(L_, -1));
+    }
+  }
+  return keys;
+}
+
+bool LuaParameterDictionary::HasKey(const string& key) const {
+  return HasKeyOfType(L_, key);
+}
+
+string LuaParameterDictionary::GetString(const string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopString(Quoted::NO);
+}
+
+string LuaParameterDictionary::PopString(Quoted quoted) const {
+  CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value.";
+  if (quoted == Quoted::YES) {
+    QuoteStringOnStack(L_);
+  }
+
+  const string value = lua_tostring(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+double LuaParameterDictionary::GetDouble(const string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopDouble();
+}
+
+double LuaParameterDictionary::PopDouble() const {
+  CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
+  const double value = lua_tonumber(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+int LuaParameterDictionary::GetInt(const string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopInt();
+}
+
+int LuaParameterDictionary::PopInt() const {
+  CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
+  const int value = lua_tointeger(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+bool LuaParameterDictionary::GetBool(const string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopBool();
+}
+
+bool LuaParameterDictionary::PopBool() const {
+  CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value.";
+  const bool value = lua_toboolean(L_, -1);
+  lua_pop(L_, 1);
+  return value;
+}
+
+std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
+    const string& key) {
+  CheckHasKeyAndReference(key);
+  GetValueFromLuaTable(L_, key);
+  return PopDictionary(reference_count_);
+}
+
+std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::PopDictionary(
+    ReferenceCount reference_count) const {
+  CheckTableIsAtTopOfStack(L_);
+  std::unique_ptr<LuaParameterDictionary> value(
+      new LuaParameterDictionary(L_, reference_count, file_resolver_));
+  // The constructor lua_xmove()s the value, no need to pop it.
+  CheckTableIsAtTopOfStack(L_);
+  return value;
+}
+
+string LuaParameterDictionary::DoToString(const string& indent) const {
+  string result = "{";
+  bool dictionary_is_empty = true;
+
+  const auto top_of_stack_to_string = [this, indent,
+                                       &dictionary_is_empty]() -> string {
+    dictionary_is_empty = false;
+
+    const int value_type = lua_type(L_, -1);
+    switch (value_type) {
+      case LUA_TBOOLEAN:
+        return PopBool() ? "true" : "false";
+        break;
+      case LUA_TSTRING:
+        return PopString(Quoted::YES);
+        break;
+      case LUA_TNUMBER: {
+        const double value = PopDouble();
+        if (std::isinf(value)) {
+          return value < 0 ? "-math.huge" : "math.huge";
+        } else {
+          return std::to_string(value);
+        }
+      } break;
+      case LUA_TTABLE: {
+        std::unique_ptr<LuaParameterDictionary> subdict(
+            PopDictionary(ReferenceCount::NO));
+        return subdict->DoToString(indent + "  ");
+      } break;
+      default:
+        LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type);
+    }
+  };
+
+  // Integer (array) keys.
+  for (int i = 1; i; ++i) {
+    GetValueFromLuaTable(L_, i);
+    if (lua_isnil(L_, -1)) {
+      lua_pop(L_, 1);
+      break;
+    }
+    result.append("\n");
+    result.append(indent);
+    result.append("  ");
+    result.append(top_of_stack_to_string());
+    result.append(",");
+  }
+
+  // String keys.
+  std::vector<string> keys = GetKeys();
+  if (!keys.empty()) {
+    std::sort(keys.begin(), keys.end());
+    for (const string& key : keys) {
+      GetValueFromLuaTable(L_, key);
+      result.append("\n");
+      result.append(indent);
+      result.append("  ");
+      result.append(key);
+      result.append(" = ");
+      result.append(top_of_stack_to_string());
+      result.append(",");
+    }
+  }
+  result.append("\n");
+  result.append(indent);
+  result.append("}");
+
+  if (dictionary_is_empty) {
+    return "{}";
+  }
+  return result;
+}
+
+string LuaParameterDictionary::ToString() const { return DoToString(""); }
+
+std::vector<double> LuaParameterDictionary::GetArrayValuesAsDoubles() {
+  std::vector<double> values;
+  GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); });
+  return values;
+}
+
+std::vector<std::unique_ptr<LuaParameterDictionary>>
+LuaParameterDictionary::GetArrayValuesAsDictionaries() {
+  std::vector<std::unique_ptr<LuaParameterDictionary>> values;
+  GetArrayValues(L_, [&values, this] {
+    values.push_back(PopDictionary(reference_count_));
+  });
+  return values;
+}
+
+std::vector<string> LuaParameterDictionary::GetArrayValuesAsStrings() {
+  std::vector<string> values;
+  GetArrayValues(L_,
+                 [&values, this] { values.push_back(PopString(Quoted::NO)); });
+  return values;
+}
+
+void LuaParameterDictionary::CheckHasKey(const string& key) const {
+  CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n"
+                     << ToString();
+}
+
+void LuaParameterDictionary::CheckHasKeyAndReference(const string& key) {
+  CheckHasKey(key);
+  reference_counts_[key]++;
+}
+
+void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() {
+  for (const auto& key : GetKeys()) {
+    CHECK_EQ(1, reference_counts_.count(key))
+        << "Key '" << key << "' was used the wrong number of times.";
+    CHECK_EQ(1, reference_counts_.at(key))
+        << "Key '" << key << "' was used the wrong number of times.";
+  }
+  reference_counts_.clear();
+}
+
+int LuaParameterDictionary::GetNonNegativeInt(const string& key) {
+  const int temp = GetInt(key);  // Will increase reference count.
+  CHECK_GE(temp, 0) << temp << " is negative.";
+  return temp;
+}
+
+// Lua function to run a script in the current Lua context. Takes the filename
+// as its only argument.
+int LuaParameterDictionary::LuaInclude(lua_State* L) {
+  CHECK_EQ(lua_gettop(L), 1);
+  CHECK(lua_isstring(L, -1)) << "include takes a filename.";
+
+  LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
+  const string basename = lua_tostring(L, -1);
+  const string filename =
+      parameter_dictionary->file_resolver_->GetFullPathOrDie(basename);
+  if (std::find(parameter_dictionary->included_files_.begin(),
+                parameter_dictionary->included_files_.end(),
+                filename) != parameter_dictionary->included_files_.end()) {
+    string error_msg = "Tried to include " + filename +
+                       " twice. Already included files in order of inclusion: ";
+    for (const string& filename : parameter_dictionary->included_files_) {
+      error_msg.append(filename);
+      error_msg.append("\n");
+    }
+    LOG(FATAL) << error_msg;
+  }
+  parameter_dictionary->included_files_.push_back(filename);
+  lua_pop(L, 1);
+  CHECK_EQ(lua_gettop(L), 0);
+
+  const string content =
+      parameter_dictionary->file_resolver_->GetFileContentOrDie(basename);
+  CheckForLuaErrors(
+      L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
+  CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0));
+
+  return lua_gettop(L);
+}
+
+// Lua function to read a file into a string.
+int LuaParameterDictionary::LuaRead(lua_State* L) {
+  CHECK_EQ(lua_gettop(L), 1);
+  CHECK(lua_isstring(L, -1)) << "read takes a filename.";
+
+  LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
+  const string file_content =
+      parameter_dictionary->file_resolver_->GetFileContentOrDie(
+          lua_tostring(L, -1));
+  lua_pushstring(L, file_content.c_str());
+  return 1;
+}
+
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/lua_parameter_dictionary.h

@@ -0,0 +1,163 @@
+/*
+ * 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_COMMON_LUA_PARAMETER_DICTIONARY_H_
+#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "cartographer/common/lua.h"
+#include "cartographer/common/port.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace common {
+
+// Resolves file paths and file content for the Lua 'read' and 'include'
+// functions. Use this to configure where those functions load other files from.
+class FileResolver {
+ public:
+  virtual ~FileResolver() {}
+  virtual string GetFullPathOrDie(const string& basename) = 0;
+  virtual string GetFileContentOrDie(const string& basename) = 0;
+};
+
+// A function that adds new Lua functions to a state. Used to extend the Lua
+// configuration in custom contexts.
+using StateExtensionFunction = std::function<void(lua_State*)>;
+
+// A parameter dictionary that gets loaded from Lua code.
+class LuaParameterDictionary {
+ public:
+  // Constructs the dictionary from a Lua Table specification.
+  LuaParameterDictionary(const string& code,
+                         std::unique_ptr<FileResolver> file_resolver,
+                         StateExtensionFunction state_extension_functon);
+
+  LuaParameterDictionary(const LuaParameterDictionary&) = delete;
+  LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete;
+
+  // Constructs a LuaParameterDictionary without reference counting.
+  static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(
+      const string& code, std::unique_ptr<FileResolver> file_resolver,
+      StateExtensionFunction state_extension_functon);
+
+  // Constructs a partial LuaParameterDictionary by extracting the dictionary
+  // with 'key' from 'code' where 'key' refers to an arbitrarily deep dictionary
+  // (e.g. "a.b.c").
+  static std::unique_ptr<LuaParameterDictionary> Partial(
+      const string& code, const string& key,
+      std::unique_ptr<FileResolver> file_resolver,
+      StateExtensionFunction state_extension_functon);
+
+  ~LuaParameterDictionary();
+
+  // Returns all available keys.
+  std::vector<string> GetKeys() const;
+
+  // Returns true if the key is in this dictionary.
+  bool HasKey(const string& key) const;
+
+  // These methods CHECK() that the 'key' exists.
+  string GetString(const string& key);
+  double GetDouble(const string& key);
+  int GetInt(const string& key);
+  bool GetBool(const string& key);
+  std::unique_ptr<LuaParameterDictionary> GetDictionary(const string& key);
+
+  // Gets an int from the dictionary and CHECK()s that it is non-negative.
+  int GetNonNegativeInt(const string& key);
+
+  // Returns a string representation for this LuaParameterDictionary.
+  string ToString() const;
+
+  // Returns the values of the keys '1', '2', '3' as the given types.
+  std::vector<double> GetArrayValuesAsDoubles();
+  std::vector<string> GetArrayValuesAsStrings();
+  std::vector<std::unique_ptr<LuaParameterDictionary>>
+  GetArrayValuesAsDictionaries();
+
+ private:
+  enum class ReferenceCount { YES, NO };
+  LuaParameterDictionary(const string& code, ReferenceCount reference_count,
+                         std::unique_ptr<FileResolver> file_resolver,
+                         StateExtensionFunction state_extension_function);
+
+  // For GetDictionary().
+  LuaParameterDictionary(lua_State* L, ReferenceCount reference_count,
+                         std::shared_ptr<FileResolver> file_resolver);
+
+  // Function that recurses to keep track of indent for ToString().
+  string DoToString(const string& indent) const;
+
+  // Pop the top of the stack and CHECKs that the type is correct.
+  double PopDouble() const;
+  int PopInt() const;
+  bool PopBool() const;
+
+  // Pop the top of the stack and CHECKs that it is a string. The returned value
+  // is either quoted to be suitable to be read back by a Lua interpretor or
+  // not.
+  enum class Quoted { YES, NO };
+  string PopString(Quoted quoted) const;
+
+  // Creates a LuaParameterDictionary from the Lua table at the top of the
+  // stack, either with or without reference counting.
+  std::unique_ptr<LuaParameterDictionary> PopDictionary(
+      ReferenceCount reference_count) const;
+
+  // CHECK() that 'key' is in the dictionary.
+  void CheckHasKey(const string& key) const;
+
+  // CHECK() that 'key' is in this dictionary and reference it as being used.
+  void CheckHasKeyAndReference(const string& key);
+
+  // If desired, this can be called in the destructor of a derived class. It
+  // will CHECK() that all keys defined in the configuration have been used
+  // exactly once and resets the reference counter.
+  void CheckAllKeysWereUsedExactlyOnceAndReset();
+
+  // Reads a file into a Lua string.
+  static int LuaRead(lua_State* L);
+
+  // Handles inclusion of other Lua files and prevents double inclusion.
+  static int LuaInclude(lua_State* L);
+
+  lua_State* L_;  // The name is by convention in the Lua World.
+  int index_into_reference_table_;
+
+  // This is shared with all the sub dictionaries.
+  const std::shared_ptr<FileResolver> file_resolver_;
+
+  // If true will check that all keys were used on destruction.
+  const ReferenceCount reference_count_;
+
+  // This is modified with every call to Get* in order to verify that all
+  // parameters are read exactly once.
+  std::map<string, int> reference_counts_;
+
+  // List of all included files in order of inclusion. Used to prevent double
+  // inclusion.
+  std::vector<string> included_files_;
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_

cartographer/cartographer/common/lua_parameter_dictionary_test.cc

@@ -0,0 +1,245 @@
+/*
+ * 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/common/lua_parameter_dictionary.h"
+
+#include <algorithm>
+#include <cmath>
+#include <memory>
+
+#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "cartographer/common/make_unique.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace common {
+namespace {
+
+std::unique_ptr<LuaParameterDictionary> MakeNonReferenceCounted(
+    const string& code) {
+  return LuaParameterDictionary::NonReferenceCounted(
+      code, common::make_unique<DummyFileResolver>(),
+      nullptr /* state_extension_function */);
+}
+
+std::unique_ptr<LuaParameterDictionary> MakePartial(const string& code,
+                                                    const string& key) {
+  return LuaParameterDictionary::Partial(
+      code, key, common::make_unique<DummyFileResolver>(),
+      nullptr /* state_extension_function */);
+}
+
+class LuaParameterDictionaryTest : public ::testing::Test {
+ protected:
+  void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) {
+    for (const string& key : dict->GetKeys()) {
+      dict->GetInt(key);
+    }
+  }
+};
+
+TEST_F(LuaParameterDictionaryTest, GetInt) {
+  auto dict = MakeDictionary("return { blah = 100 }");
+  ASSERT_EQ(dict->GetInt("blah"), 100);
+}
+
+TEST_F(LuaParameterDictionaryTest, GetString) {
+  auto dict = MakeDictionary("return { blah = 'is_a_string' }\n");
+  ASSERT_EQ(dict->GetString("blah"), "is_a_string");
+}
+
+TEST_F(LuaParameterDictionaryTest, GetDouble) {
+  auto dict = MakeDictionary("return { blah = 3.1415 }");
+  ASSERT_DOUBLE_EQ(dict->GetDouble("blah"), 3.1415);
+}
+
+TEST_F(LuaParameterDictionaryTest, GetBoolTrue) {
+  auto dict = MakeDictionary("return { blah = true }");
+  ASSERT_TRUE(dict->GetBool("blah"));
+}
+
+TEST_F(LuaParameterDictionaryTest, GetBoolFalse) {
+  auto dict = MakeDictionary("return { blah = false }");
+  ASSERT_FALSE(dict->GetBool("blah"));
+}
+
+TEST_F(LuaParameterDictionaryTest, GetDictionary) {
+  auto dict =
+      MakeDictionary("return { blah = { blue = 100, red = 200 }, fasel = 10 }");
+
+  std::unique_ptr<LuaParameterDictionary> sub_dict(dict->GetDictionary("blah"));
+  std::vector<string> keys = sub_dict->GetKeys();
+  ASSERT_EQ(keys.size(), 2);
+  std::sort(keys.begin(), keys.end());
+  ASSERT_EQ(keys[0], "blue");
+  ASSERT_EQ(keys[1], "red");
+  ASSERT_TRUE(sub_dict->HasKey("blue"));
+  ASSERT_TRUE(sub_dict->HasKey("red"));
+  ASSERT_EQ(sub_dict->GetInt("blue"), 100);
+  ASSERT_EQ(sub_dict->GetInt("red"), 200);
+
+  ASSERT_EQ(dict->GetString("fasel"), "10");
+}
+
+TEST_F(LuaParameterDictionaryTest, GetKeys) {
+  auto dict = MakeDictionary("return { blah = 100, blah1 = 200}");
+
+  std::vector<string> keys = dict->GetKeys();
+  ASSERT_EQ(keys.size(), 2);
+  std::sort(keys.begin(), keys.end());
+  ASSERT_EQ(keys[0], "blah");
+  ASSERT_EQ(keys[1], "blah1");
+
+  ReferenceAllKeysAsIntegers(dict.get());
+}
+
+TEST_F(LuaParameterDictionaryTest, NonExistingKey) {
+  auto dict = MakeDictionary("return { blah = 100 }");
+  ReferenceAllKeysAsIntegers(dict.get());
+  ASSERT_DEATH(dict->GetInt("blah_fasel"), "Key.* not in dictionary.");
+}
+
+TEST_F(LuaParameterDictionaryTest, UintNegative) {
+  auto dict = MakeDictionary("return { blah = -100}");
+  ASSERT_DEATH(dict->GetNonNegativeInt("blah"), ".*-100 is negative.");
+  ReferenceAllKeysAsIntegers(dict.get());
+}
+
+TEST_F(LuaParameterDictionaryTest, ToString) {
+  auto dict = MakeDictionary(R"(return {
+  ceta = { yolo = "hurray" },
+  fasel = 1234.456786,
+  fasel1 = -math.huge,
+  fasel2 = math.huge,
+  blubber = 123,
+  blub = 'hello',
+  alpha = true,
+  alpha1 = false,
+  })");
+
+  const string golden = R"({
+  alpha = true,
+  alpha1 = false,
+  blub = "hello",
+  blubber = 123.000000,
+  ceta = {
+    yolo = "hurray",
+  },
+  fasel = 1234.456786,
+  fasel1 = -math.huge,
+  fasel2 = math.huge,
+})";
+  EXPECT_EQ(golden, dict->ToString());
+
+  auto dict1 = MakeDictionary("return " + dict->ToString());
+
+  EXPECT_EQ(dict1->GetBool("alpha"), true);
+  EXPECT_EQ(dict1->GetBool("alpha1"), false);
+  EXPECT_EQ(dict1->GetInt("blubber"), 123);
+  EXPECT_EQ(dict1->GetString("blub"), "hello");
+  EXPECT_EQ(dict1->GetDictionary("ceta")->GetString("yolo"), "hurray");
+  EXPECT_NEAR(dict1->GetDouble("fasel"), 1234.456786, 1e-3);
+  EXPECT_TRUE(std::isinf(-dict1->GetDouble("fasel1")));
+  EXPECT_TRUE(std::isinf(dict1->GetDouble("fasel2")));
+
+  EXPECT_EQ(dict->GetBool("alpha"), true);
+  EXPECT_EQ(dict->GetBool("alpha1"), false);
+  EXPECT_EQ(dict->GetInt("blubber"), 123);
+  EXPECT_EQ(dict->GetString("blub"), "hello");
+  EXPECT_EQ(dict->GetDictionary("ceta")->GetString("yolo"), "hurray");
+  EXPECT_NEAR(dict->GetDouble("fasel"), 1234.456786, 1e-3);
+  EXPECT_TRUE(std::isinf(-dict->GetDouble("fasel1")));
+  EXPECT_TRUE(std::isinf(dict->GetDouble("fasel2")));
+}
+
+TEST_F(LuaParameterDictionaryTest, ToStringForArrays) {
+  auto dict = MakeNonReferenceCounted(
+      R"(return {
+      "blub", 3, 3.1,
+      foo = "ups",
+  })");
+
+  const string golden = R"({
+  "blub",
+  3.000000,
+  3.100000,
+  foo = "ups",
+})";
+  EXPECT_EQ(golden, dict->ToString());
+}
+
+TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) {
+  auto dict = MakeDictionary("return { 'a', 'b', 'c' }");
+  EXPECT_EQ(0, dict->GetKeys().size());
+  const std::vector<string> values = dict->GetArrayValuesAsStrings();
+  EXPECT_EQ(3, values.size());
+  EXPECT_EQ("a", values[0]);
+  EXPECT_EQ("b", values[1]);
+  EXPECT_EQ("c", values[2]);
+}
+
+TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDoubles) {
+  auto dict = MakeDictionary("return { 1., 2., 3. }");
+  EXPECT_EQ(0, dict->GetKeys().size());
+  const std::vector<double> values = dict->GetArrayValuesAsDoubles();
+  EXPECT_EQ(3, values.size());
+  EXPECT_NEAR(1., values[0], 1e-7);
+  EXPECT_NEAR(2., values[1], 1e-7);
+  EXPECT_NEAR(3., values[2], 1e-7);
+}
+
+TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDictionaries) {
+  auto dict = MakeDictionary("return { { a = 1 }, { b = 3 } }");
+  EXPECT_EQ(0, dict->GetKeys().size());
+  const std::vector<std::unique_ptr<LuaParameterDictionary>> values =
+      dict->GetArrayValuesAsDictionaries();
+  EXPECT_EQ(2, values.size());
+  EXPECT_EQ(1., values[0]->GetInt("a"));
+  EXPECT_EQ(3., values[1]->GetInt("b"));
+}
+
+TEST_F(LuaParameterDictionaryTest, TestChooseTrue) {
+  auto dict = MakeDictionary("return { a = choose(true, 1, 0) }");
+  EXPECT_EQ(1, dict->GetInt("a"));
+}
+
+TEST_F(LuaParameterDictionaryTest, TestChoseFalse) {
+  auto dict = MakeDictionary("return { a = choose(false, 1, 0) }");
+  EXPECT_EQ(0, dict->GetInt("a"));
+}
+
+TEST_F(LuaParameterDictionaryTest, TestChooseInvalidArgument) {
+  EXPECT_DEATH(MakeDictionary("return { a = choose('truish', 1, 0) }"),
+               "condition is not a boolean value.");
+}
+
+TEST_F(LuaParameterDictionaryTest, Partial) {
+  auto partial_dictionary =
+      MakePartial("return { blah = { blue = { red = 200 } } }", "blah.blue");
+  EXPECT_EQ(200, partial_dictionary->GetInt("red"));
+}
+
+TEST_F(LuaParameterDictionaryTest, PartialIsReferenceCounted) {
+  auto partial_dictionary =
+      MakePartial("return { blah = { blue = { red = 200 } } }", "blah.blue");
+  ASSERT_DEATH(partial_dictionary.reset(),
+               ".*Key 'red' was used the wrong number of times..*");
+  partial_dictionary->GetInt("red");
+}
+
+}  // namespace
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/lua_parameter_dictionary_test_helpers.h

@@ -0,0 +1,58 @@
+/*
+ * 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_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
+#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
+
+#include <memory>
+#include <string>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/port.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace common {
+
+class DummyFileResolver : public FileResolver {
+ public:
+  DummyFileResolver() {}
+
+  DummyFileResolver(const DummyFileResolver&) = delete;
+  DummyFileResolver& operator=(const DummyFileResolver&) = delete;
+
+  ~DummyFileResolver() override {}
+
+  string GetFileContentOrDie(const string& unused_basename) override {
+    LOG(FATAL) << "Not implemented";
+  }
+
+  string GetFullPathOrDie(const string& unused_basename) override {
+    LOG(FATAL) << "Not implemented";
+  }
+};
+
+std::unique_ptr<LuaParameterDictionary> MakeDictionary(const string& code) {
+  return common::make_unique<LuaParameterDictionary>(
+      code, std::unique_ptr<DummyFileResolver>(new DummyFileResolver()),
+      nullptr /* state_extension_function */);
+}
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_

cartographer/cartographer/common/make_unique.h

@@ -0,0 +1,62 @@
+/*
+ * 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_COMMON_MAKE_UNIQUE_H_
+#define CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_
+
+#include <cstddef>
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+namespace cartographer {
+namespace common {
+
+// Implementation of c++14's std::make_unique, taken from
+// https://isocpp.org/files/papers/N3656.txt
+template <class T>
+struct _Unique_if {
+  typedef std::unique_ptr<T> _Single_object;
+};
+
+template <class T>
+struct _Unique_if<T[]> {
+  typedef std::unique_ptr<T[]> _Unknown_bound;
+};
+
+template <class T, size_t N>
+struct _Unique_if<T[N]> {
+  typedef void _Known_bound;
+};
+
+template <class T, class... Args>
+typename _Unique_if<T>::_Single_object make_unique(Args&&... args) {
+  return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
+}
+
+template <class T>
+typename _Unique_if<T>::_Unknown_bound make_unique(size_t n) {
+  typedef typename std::remove_extent<T>::type U;
+  return std::unique_ptr<T>(new U[n]());
+}
+
+template <class T, class... Args>
+typename _Unique_if<T>::_Known_bound make_unique(Args&&...) = delete;
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_

cartographer/cartographer/common/math.h

@@ -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.
+ */
+
+#ifndef CARTOGRAPHER_COMMON_MATH_H_
+#define CARTOGRAPHER_COMMON_MATH_H_
+
+#include <cmath>
+#include <vector>
+
+#include "Eigen/Core"
+#include "cartographer/common/port.h"
+#include "ceres/ceres.h"
+
+namespace cartographer {
+namespace common {
+
+// Clamps 'value' to be in the range ['min', 'max'].
+template <typename T>
+T Clamp(const T value, const T min, const T max) {
+  if (value > max) {
+    return max;
+  }
+  if (value < min) {
+    return min;
+  }
+  return value;
+}
+
+// Calculates 'base'^'exponent'.
+template <typename T>
+constexpr T Power(T base, int exponent) {
+  return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
+}
+
+// Calculates a^2.
+template <typename T>
+constexpr T Pow2(T a) {
+  return Power(a, 2);
+}
+
+// Calculates the real part of the square root of 'a'. This is helpful when
+// rounding errors generate a small negative argument. Otherwise std::sqrt
+// returns NaN if its argument is negative.
+template <typename T>
+constexpr T RealSqrt(T a) {
+  return sqrt(std::max(T(0.), a));
+}
+
+// Converts from degrees to radians.
+constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
+
+// Converts form radians to degrees.
+constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
+
+// Bring the 'difference' between two angles into [-pi; pi].
+template <typename T>
+T NormalizeAngleDifference(T difference) {
+  while (difference > M_PI) {
+    difference -= T(2. * M_PI);
+  }
+  while (difference < -M_PI) {
+    difference += T(2. * M_PI);
+  }
+  return difference;
+}
+
+template <typename T>
+T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
+  return ceres::atan2(vector.y(), vector.x());
+}
+
+// Computes (A/'scale')^{-1/2} for A being symmetric, positive-semidefinite.
+// Eigenvalues of 'A' are clamped to be at least 'lower_eigenvalue_bound'.
+template <int N>
+Eigen::Matrix<double, N, N> ComputeSpdMatrixSqrtInverse(
+    const Eigen::Matrix<double, N, N>& A, const double scale,
+    const double lower_eigenvalue_bound) {
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>>
+      covariance_eigen_solver(A / scale);
+  if (covariance_eigen_solver.info() != Eigen::Success) {
+    LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A;
+    return Eigen::Matrix<double, N, N>::Identity();
+  }
+  // Since we compute the inverse, we do not allow smaller values to avoid
+  // infinity and NaN.
+  const double relative_lower_bound = lower_eigenvalue_bound / scale;
+  return covariance_eigen_solver.eigenvectors() *
+         covariance_eigen_solver.eigenvalues()
+             .cwiseMax(relative_lower_bound)
+             .cwiseInverse()
+             .cwiseSqrt()
+             .asDiagonal() *
+         covariance_eigen_solver.eigenvectors().inverse();
+}
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_MATH_H_

cartographer/cartographer/common/math_test.cc

@@ -0,0 +1,61 @@
+/*
+ * 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/common/math.h"
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace common {
+namespace {
+
+TEST(MathTest, testPower) {
+  EXPECT_EQ(0., Power(0, 42));
+  EXPECT_EQ(1., Power(0, 0));
+  EXPECT_EQ(1., Power(1, 0));
+  EXPECT_EQ(1., Power(1, 42));
+  EXPECT_EQ(4., Power(2, 2));
+}
+
+TEST(MathTest, testPow2) {
+  EXPECT_EQ(0., Pow2(0));
+  EXPECT_EQ(1., Pow2(1));
+  EXPECT_EQ(4., Pow2(2));
+  EXPECT_EQ(49., Pow2(7));
+}
+
+TEST(MathTest, testDeg2rad) {
+  EXPECT_NEAR(M_PI, DegToRad(180.), 1e-9);
+  EXPECT_NEAR(2. * M_PI, DegToRad(360. - 1e-9), 1e-6);
+}
+
+TEST(MathTest, testRad2deg) {
+  EXPECT_NEAR(180., RadToDeg(M_PI), 1e-9);
+  EXPECT_NEAR(360., RadToDeg(2. * M_PI - 1e-9), 1e-6);
+}
+
+TEST(MathTest, testNormalizeAngleDifference) {
+  EXPECT_NEAR(0., NormalizeAngleDifference(0.), 1e-9);
+  EXPECT_NEAR(M_PI, NormalizeAngleDifference(M_PI), 1e-9);
+  EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-M_PI), 1e-9);
+  EXPECT_NEAR(0., NormalizeAngleDifference(2. * M_PI), 1e-9);
+  EXPECT_NEAR(M_PI, NormalizeAngleDifference(5. * M_PI), 1e-9);
+  EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-5. * M_PI), 1e-9);
+}
+
+}  // namespace
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/mutex.h

@@ -0,0 +1,100 @@
+/*
+ * 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_COMMON_MUTEX_H_
+#define CARTOGRAPHER_COMMON_MUTEX_H_
+
+#include <condition_variable>
+#include <mutex>
+
+#include "cartographer/common/time.h"
+
+namespace cartographer {
+namespace common {
+
+// Enable thread safety attributes only with clang.
+// The attributes can be safely erased when compiling with other compilers.
+#if defined(__SUPPORT_TS_ANNOTATION__) || defined(__clang__)
+#define THREAD_ANNOTATION_ATTRIBUTE__(x) __attribute__((x))
+#else
+#define THREAD_ANNOTATION_ATTRIBUTE__(x)  // no-op
+#endif
+
+#define CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(capability(x))
+
+#define SCOPED_CAPABILITY THREAD_ANNOTATION_ATTRIBUTE__(scoped_lockable)
+
+#define GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(guarded_by(x))
+
+#define PT_GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(pt_guarded_by(x))
+
+#define REQUIRES(...) \
+  THREAD_ANNOTATION_ATTRIBUTE__(requires_capability(__VA_ARGS__))
+
+#define ACQUIRE(...) \
+  THREAD_ANNOTATION_ATTRIBUTE__(acquire_capability(__VA_ARGS__))
+
+#define RELEASE(...) \
+  THREAD_ANNOTATION_ATTRIBUTE__(release_capability(__VA_ARGS__))
+
+#define EXCLUDES(...) THREAD_ANNOTATION_ATTRIBUTE__(locks_excluded(__VA_ARGS__))
+
+#define NO_THREAD_SAFETY_ANALYSIS \
+  THREAD_ANNOTATION_ATTRIBUTE__(no_thread_safety_analysis)
+
+// Defines an annotated mutex that can only be locked through its scoped locker
+// implementation.
+class CAPABILITY("mutex") Mutex {
+ public:
+  // A RAII class that acquires a mutex in its constructor, and
+  // releases it in its destructor. It also implements waiting functionality on
+  // conditions that get checked whenever the mutex is released.
+  class SCOPED_CAPABILITY Locker {
+   public:
+    Locker(Mutex* mutex) ACQUIRE(mutex) : mutex_(mutex), lock_(mutex->mutex_) {}
+
+    ~Locker() RELEASE() {
+      lock_.unlock();
+      mutex_->condition_.notify_all();
+    }
+
+    template <typename Predicate>
+    void Await(Predicate predicate) REQUIRES(this) {
+      mutex_->condition_.wait(lock_, predicate);
+    }
+
+    template <typename Predicate>
+    bool AwaitWithTimeout(Predicate predicate, common::Duration timeout)
+        REQUIRES(this) {
+      return mutex_->condition_.wait_for(lock_, timeout, predicate);
+    }
+
+   private:
+    Mutex* mutex_;
+    std::unique_lock<std::mutex> lock_;
+  };
+
+ private:
+  std::condition_variable condition_;
+  std::mutex mutex_;
+};
+
+using MutexLocker = Mutex::Locker;
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_MUTEX_H_

cartographer/cartographer/common/ordered_multi_queue.h

@@ -0,0 +1,179 @@
+/*
+ * 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_COMMON_ORDERED_MULTI_QUEUE_H_
+#define CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
+
+#include <algorithm>
+#include <map>
+#include <memory>
+
+#include "cartographer/common/blocking_queue.h"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/port.h"
+
+namespace cartographer {
+namespace common {
+
+// Number of items that can be queued up before we LOG(WARNING).
+const int kMaxQueueSize = 500;
+
+// Maintains multiple queues of sorted values and dispatches merge sorted
+// values. This class is thread-compatible.
+template <typename QueueKeyType, typename SortKeyType, typename ValueType>
+class OrderedMultiQueue {
+ public:
+  using Callback = std::function<void(std::unique_ptr<ValueType>)>;
+
+  // Will wait to see at least one value for each 'expected_queue_keys' before
+  // dispatching the next smallest value across all queues.
+  explicit OrderedMultiQueue(const SortKeyType min_sort_key = SortKeyType())
+      : last_dispatched_key_(min_sort_key) {}
+
+  ~OrderedMultiQueue() {}
+
+  void AddQueue(const QueueKeyType& queue_key, Callback callback) {
+    CHECK(FindOrNull(queue_key) == nullptr);
+    queues_[queue_key].callback = callback;
+  }
+
+  void MarkQueueAsFinished(const QueueKeyType& queue_key) {
+    auto& queue = FindOrDie(queue_key);
+    CHECK(!queue.finished);
+    queue.finished = true;
+    Dispatch();
+  }
+
+  bool HasQueue(const QueueKeyType& queue_key) {
+    return queues_.count(queue_key) != 0;
+  }
+
+  void Add(const QueueKeyType& queue_key, const SortKeyType& sort_key,
+           std::unique_ptr<ValueType> value) {
+    auto* queue = FindOrNull(queue_key);
+    if (queue == nullptr) {
+      // TODO(damonkohler): This will not work for every value of "queue_key".
+      LOG_EVERY_N(WARNING, 60) << "Ignored value for queue: '" << queue_key
+                               << "'";
+      return;
+    }
+    queue->queue.Push(
+        common::make_unique<KeyValuePair>(sort_key, std::move(value)));
+    Dispatch();
+  }
+
+  // Dispatches all remaining values in sorted order and removes the underlying
+  // queues.
+  void Flush() {
+    std::vector<QueueKeyType> unfinished_queues;
+    for (auto& entry : queues_) {
+      if (!entry.second.finished) {
+        unfinished_queues.push_back(entry.first);
+      }
+    }
+    for (auto& unfinished_queue : unfinished_queues) {
+      MarkQueueAsFinished(unfinished_queue);
+    }
+  }
+
+  // Returns the number of available values associated with 'queue_key'.
+  int num_available(const QueueKeyType& queue_key) {
+    return FindOrDie(queue_key).queue.Size();
+  }
+
+ private:
+  struct KeyValuePair {
+    KeyValuePair(const SortKeyType& sort_key, std::unique_ptr<ValueType> value)
+        : sort_key(sort_key), value(std::move(value)) {}
+    SortKeyType sort_key;
+    std::unique_ptr<ValueType> value;
+  };
+
+  struct Queue {
+    common::BlockingQueue<std::unique_ptr<KeyValuePair>> queue;
+    Callback callback;
+    bool finished = false;
+  };
+
+  // Returns the queue with 'key' or LOG(FATAL).
+  Queue& FindOrDie(const QueueKeyType& key) {
+    auto it = queues_.find(key);
+    CHECK(it != queues_.end()) << "Did not find '" << key << "'.";
+    return it->second;
+  }
+
+  // Returns the queue with 'key' or nullptr.
+  Queue* FindOrNull(const QueueKeyType& key) {
+    auto it = queues_.find(key);
+    if (it == queues_.end()) {
+      return nullptr;
+    }
+    return &it->second;
+  }
+
+  void Dispatch() {
+    while (true) {
+      Queue* next_queue = nullptr;
+      const KeyValuePair* next_key_value_pair = nullptr;
+      for (auto it = queues_.begin(); it != queues_.end();) {
+        auto& queue = it->second.queue;
+        const auto* key_value_pair = queue.template Peek<KeyValuePair>();
+        if (key_value_pair == nullptr) {
+          if (it->second.finished) {
+            queues_.erase(it++);
+            continue;
+          }
+          CannotMakeProgress();
+          return;
+        }
+        if (next_key_value_pair == nullptr ||
+            std::forward_as_tuple(key_value_pair->sort_key, it->first) <
+                std::forward_as_tuple(next_key_value_pair->sort_key,
+                                      it->first)) {
+          next_key_value_pair = key_value_pair;
+          next_queue = &it->second;
+        }
+        CHECK_LE(last_dispatched_key_, next_key_value_pair->sort_key)
+            << "Non-sorted values added to queue: '" << it->first << "'";
+        ++it;
+      }
+      if (next_key_value_pair == nullptr) {
+        CHECK(queues_.empty());
+        return;
+      }
+      last_dispatched_key_ = next_key_value_pair->sort_key;
+      next_queue->callback(std::move(next_queue->queue.Pop()->value));
+    }
+  }
+
+  // Called when not all necessary queues are filled to dispatch messages.
+  void CannotMakeProgress() {
+    for (auto& entry : queues_) {
+      LOG_IF_EVERY_N(WARNING, entry.second.queue.Size() > kMaxQueueSize, 60)
+          << "Queue " << entry.first << " exceeds maximum size.";
+    }
+  }
+
+  // Used to verify that values are dispatched in sorted order.
+  SortKeyType last_dispatched_key_;
+
+  std::map<QueueKeyType, Queue> queues_;
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_

cartographer/cartographer/common/ordered_multi_queue_test.cc

@@ -0,0 +1,88 @@
+/*
+ * 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/common/ordered_multi_queue.h"
+
+#include <vector>
+
+#include "cartographer/common/make_unique.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace common {
+namespace {
+
+TEST(OrderedMultiQueue, Ordering) {
+  std::vector<int> values;
+  OrderedMultiQueue<int, int, int> queue;
+  for (int i : {1, 2, 3}) {
+    queue.AddQueue(i, [&values](std::unique_ptr<int> value) {
+      if (!values.empty()) {
+        EXPECT_GT(*value, values.back());
+      }
+      values.push_back(*value);
+    });
+  }
+  queue.Add(1, 4, common::make_unique<int>(4));
+  queue.Add(1, 5, common::make_unique<int>(5));
+  queue.Add(1, 6, common::make_unique<int>(6));
+  EXPECT_TRUE(values.empty());
+  queue.Add(2, 1, common::make_unique<int>(1));
+  EXPECT_TRUE(values.empty());
+  queue.Add(3, 2, common::make_unique<int>(2));
+  EXPECT_EQ(values.size(), 1);
+  queue.Add(2, 3, common::make_unique<int>(3));
+  EXPECT_EQ(values.size(), 2);
+  queue.Add(2, 7, common::make_unique<int>(7));
+  queue.Add(3, 8, common::make_unique<int>(8));
+  queue.Flush();
+
+  EXPECT_EQ(8, values.size());
+  for (size_t i = 0; i < values.size(); ++i) {
+    EXPECT_EQ(i + 1, values[i]);
+  }
+}
+
+TEST(OrderedMultiQueue, MarkQueueAsFinished) {
+  std::vector<int> values;
+  OrderedMultiQueue<int, int, int> queue;
+  for (int i : {1, 2, 3}) {
+    queue.AddQueue(i, [&values](std::unique_ptr<int> value) {
+      if (!values.empty()) {
+        EXPECT_GT(*value, values.back());
+      }
+      values.push_back(*value);
+    });
+  }
+  queue.Add(1, 1, common::make_unique<int>(1));
+  queue.Add(1, 2, common::make_unique<int>(2));
+  queue.Add(1, 3, common::make_unique<int>(3));
+  EXPECT_TRUE(values.empty());
+  queue.MarkQueueAsFinished(1);
+  EXPECT_TRUE(values.empty());
+  queue.MarkQueueAsFinished(2);
+  EXPECT_TRUE(values.empty());
+  queue.MarkQueueAsFinished(3);
+
+  EXPECT_EQ(3, values.size());
+  for (size_t i = 0; i < values.size(); ++i) {
+    EXPECT_EQ(i + 1, values[i]);
+  }
+}
+
+}  // namespace
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/port.h

@@ -0,0 +1,71 @@
+/*
+ * 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_COMMON_PORT_H_
+#define CARTOGRAPHER_COMMON_PORT_H_
+
+#include <cinttypes>
+#include <cmath>
+#include <string>
+
+#include <boost/iostreams/device/back_inserter.hpp>
+#include <boost/iostreams/filter/gzip.hpp>
+#include <boost/iostreams/filtering_stream.hpp>
+
+using int8 = int8_t;
+using int16 = int16_t;
+using int32 = int32_t;
+using int64 = int64_t;
+using uint8 = uint8_t;
+using uint16 = uint16_t;
+using uint32 = uint32_t;
+using uint64 = uint64_t;
+
+using std::string;
+
+namespace cartographer {
+namespace common {
+
+inline int RoundToInt(const float x) { return std::lround(x); }
+
+inline int RoundToInt(const double x) { return std::lround(x); }
+
+inline int64 RoundToInt64(const float x) { return std::lround(x); }
+
+inline int64 RoundToInt64(const double x) { return std::lround(x); }
+
+inline void FastGzipString(const string& uncompressed, string* compressed) {
+  boost::iostreams::filtering_ostream out;
+  out.push(
+      boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));
+  out.push(boost::iostreams::back_inserter(*compressed));
+  boost::iostreams::write(out,
+                          reinterpret_cast<const char*>(uncompressed.data()),
+                          uncompressed.size());
+}
+
+inline void FastGunzipString(const string& compressed, string* decompressed) {
+  boost::iostreams::filtering_ostream out;
+  out.push(boost::iostreams::gzip_decompressor());
+  out.push(boost::iostreams::back_inserter(*decompressed));
+  boost::iostreams::write(out, reinterpret_cast<const char*>(compressed.data()),
+                          compressed.size());
+}
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_PORT_H_

cartographer/cartographer/common/proto/CMakeLists.txt

@@ -0,0 +1,18 @@
+# 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.
+
+google_proto_library(common_proto_ceres_solver_options
+  SRCS
+    ceres_solver_options.proto
+)

cartographer/cartographer/common/proto/ceres_solver_options.proto

@@ -0,0 +1,25 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.common.proto;
+
+message CeresSolverOptions {
+  // Configure the Ceres solver. See the Ceres documentation for more
+  // information: https://code.google.com/p/ceres-solver/
+  optional bool use_nonmonotonic_steps = 1;
+  optional int32 max_num_iterations = 2;
+  optional int32 num_threads = 3;
+}

cartographer/cartographer/common/thread_pool.cc

@@ -0,0 +1,81 @@
+/*
+ * 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/common/thread_pool.h"
+
+#include <unistd.h>
+#include <algorithm>
+#include <chrono>
+#include <numeric>
+
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace common {
+
+ThreadPool::ThreadPool(int num_threads) {
+  MutexLocker locker(&mutex_);
+  for (int i = 0; i != num_threads; ++i) {
+    pool_.emplace_back([this]() { ThreadPool::DoWork(); });
+  }
+}
+
+ThreadPool::~ThreadPool() {
+  {
+    MutexLocker locker(&mutex_);
+    CHECK(running_);
+    running_ = false;
+    CHECK_EQ(work_queue_.size(), 0);
+  }
+  for (std::thread& thread : pool_) {
+    thread.join();
+  }
+}
+
+void ThreadPool::Schedule(std::function<void()> work_item) {
+  MutexLocker locker(&mutex_);
+  CHECK(running_);
+  work_queue_.push_back(work_item);
+}
+
+void ThreadPool::DoWork() {
+#ifdef __linux__
+  // This changes the per-thread nice level of the current thread on Linux. We
+  // do this so that the background work done by the thread pool is not taking
+  // away CPU resources from more important foreground threads.
+  CHECK_NE(nice(10), -1);
+#endif
+  for (;;) {
+    std::function<void()> work_item;
+    {
+      MutexLocker locker(&mutex_);
+      locker.Await([this]() REQUIRES(mutex_) {
+        return !work_queue_.empty() || !running_;
+      });
+      if (!work_queue_.empty()) {
+        work_item = work_queue_.front();
+        work_queue_.pop_front();
+      } else if (!running_) {
+        return;
+      }
+    }
+    CHECK(work_item);
+    work_item();
+  }
+}
+
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/thread_pool.h

@@ -0,0 +1,57 @@
+/*
+ * 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_COMMON_THREAD_POOL_H_
+#define CARTOGRAPHER_COMMON_THREAD_POOL_H_
+
+#include <deque>
+#include <functional>
+#include <thread>
+#include <vector>
+
+#include "cartographer/common/mutex.h"
+
+namespace cartographer {
+namespace common {
+
+// 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 {
+ public:
+  explicit ThreadPool(int num_threads);
+  ~ThreadPool();
+
+  ThreadPool(const ThreadPool&) = delete;
+  ThreadPool& operator=(const ThreadPool&) = delete;
+
+  void Schedule(std::function<void()> work_item);
+
+ private:
+  void DoWork();
+
+  Mutex mutex_;
+  bool running_ GUARDED_BY(mutex_) = true;
+  std::vector<std::thread> pool_ GUARDED_BY(mutex_);
+  std::deque<std::function<void()>> work_queue_ GUARDED_BY(mutex_);
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_THREAD_POOL_H_

cartographer/cartographer/common/time.cc

@@ -0,0 +1,44 @@
+/*
+ * 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/common/time.h"
+
+#include <string>
+
+namespace cartographer {
+namespace common {
+
+Duration FromSeconds(const double seconds) {
+  return Duration(static_cast<int64>(1e7 * seconds));
+}
+
+double ToSeconds(const Duration duration) { return duration.count() * 1e-7; }
+
+Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); }
+
+int64 ToUniversal(const Time time) { return time.time_since_epoch().count(); }
+
+std::ostream& operator<<(std::ostream& os, const Time time) {
+  os << std::to_string(ToUniversal(time));
+  return os;
+}
+
+common::Duration FromMilliseconds(int64 milliseconds) {
+  return common::Duration(milliseconds * 10000);
+}
+
+}  // namespace common
+}  // namespace cartographer

cartographer/cartographer/common/time.h

@@ -0,0 +1,62 @@
+/*
+ * 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_COMMON_TIME_H_
+#define CARTOGRAPHER_COMMON_TIME_H_
+
+#include <chrono>
+#include <ostream>
+#include <ratio>
+
+#include "cartographer/common/port.h"
+
+namespace cartographer {
+namespace common {
+
+struct UniversalTimeScaleClock {
+  using rep = int64;
+  using period = std::ratio<1, 10000000>;
+  using duration = std::chrono::duration<rep, period>;
+  using time_point = std::chrono::time_point<UniversalTimeScaleClock>;
+  static constexpr bool is_steady = true;
+};
+
+// Represents Universal Time Scale durations and timestamps which are 64-bit
+// integers representing the 100 nanosecond ticks since the Epoch which is
+// January 1, 1 at the start of day in UTC.
+using Duration = UniversalTimeScaleClock::duration;
+using Time = UniversalTimeScaleClock::time_point;
+
+// Convenience functions to create common::Durations.
+Duration FromSeconds(double seconds);
+Duration FromMilliseconds(int64 milliseconds);
+
+// Returns the given duration in seconds.
+double ToSeconds(Duration duration);
+
+// Creates a time from a Universal Time Scale.
+Time FromUniversal(int64 ticks);
+
+// Outputs the Universal Time Scale timestamp for a given Time.
+int64 ToUniversal(Time time);
+
+// For logging and unit tests, outputs the timestamp integer.
+std::ostream& operator<<(std::ostream& os, Time time);
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_TIME_H_

cartographer/cartographer/kalman_filter/CMakeLists.txt

@@ -0,0 +1,87 @@
+# 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.
+
+add_subdirectory("proto")
+
+google_library(kalman_filter_gaussian_distribution
+  USES_EIGEN
+  HDRS
+    gaussian_distribution.h
+)
+
+google_library(kalman_filter_odometry_state_tracker
+  SRCS
+    odometry_state_tracker.cc
+  HDRS
+    odometry_state_tracker.h
+  DEPENDS
+    common_time
+    transform_rigid_transform
+)
+
+google_library(kalman_filter_pose_tracker
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    pose_tracker.cc
+  HDRS
+    pose_tracker.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_math
+    common_port
+    common_time
+    kalman_filter_gaussian_distribution
+    kalman_filter_odometry_state_tracker
+    kalman_filter_proto_pose_tracker_options
+    kalman_filter_unscented_kalman_filter
+    sensor_proto_sensor
+    transform_transform
+)
+
+google_library(kalman_filter_unscented_kalman_filter
+  USES_CERES
+  USES_EIGEN
+  HDRS
+    unscented_kalman_filter.h
+  DEPENDS
+    kalman_filter_gaussian_distribution
+)
+
+google_test(kalman_filter_gaussian_distribution_test
+  SRCS
+    gaussian_distribution_test.cc
+  DEPENDS
+    kalman_filter_gaussian_distribution
+)
+
+google_test(kalman_filter_pose_tracker_test
+  SRCS
+    pose_tracker_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_lua_parameter_dictionary_test_helpers
+    common_make_unique
+    kalman_filter_pose_tracker
+    transform_rigid_transform
+    transform_rigid_transform_test_helpers
+)
+
+google_test(kalman_filter_unscented_kalman_filter_test
+  SRCS
+    unscented_kalman_filter_test.cc
+  DEPENDS
+    kalman_filter_gaussian_distribution
+    kalman_filter_unscented_kalman_filter
+)

cartographer/cartographer/kalman_filter/gaussian_distribution.h

@@ -0,0 +1,60 @@
+/*
+ * 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_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
+#define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
+
+#include "Eigen/Cholesky"
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+
+namespace cartographer {
+namespace kalman_filter {
+
+template <typename T, int N>
+class GaussianDistribution {
+ public:
+  GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean,
+                       const Eigen::Matrix<T, N, N>& covariance)
+      : mean_(mean), covariance_(covariance) {}
+
+  const Eigen::Matrix<T, N, 1>& GetMean() const { return mean_; }
+
+  const Eigen::Matrix<T, N, N>& GetCovariance() const { return covariance_; }
+
+ private:
+  Eigen::Matrix<T, N, 1> mean_;
+  Eigen::Matrix<T, N, N> covariance_;
+};
+
+template <typename T, int N>
+GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs,
+                                     const GaussianDistribution<T, N>& rhs) {
+  return GaussianDistribution<T, N>(lhs.GetMean() + rhs.GetMean(),
+                                    lhs.GetCovariance() + rhs.GetCovariance());
+}
+
+template <typename T, int N, int M>
+GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs,
+                                     const GaussianDistribution<T, M>& rhs) {
+  return GaussianDistribution<T, N>(
+      lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose());
+}
+
+}  // namespace kalman_filter
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_

cartographer/cartographer/kalman_filter/gaussian_distribution_test.cc

@@ -0,0 +1,37 @@
+/*
+ * 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/kalman_filter/gaussian_distribution.h"
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace kalman_filter {
+namespace {
+
+TEST(GaussianDistributionTest, testConstructor) {
+  Eigen::Matrix2d covariance;
+  covariance << 1., 2., 3., 4.;
+  GaussianDistribution<double, 2> distribution(Eigen::Vector2d(0., 1.),
+                                               covariance);
+  EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);
+  EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9);
+  EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);
+}
+
+}  // namespace
+}  // namespace kalman_filter
+}  // namespace cartographer

cartographer/cartographer/kalman_filter/odometry_state_tracker.cc

@@ -0,0 +1,54 @@
+/*
+ * 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/kalman_filter/odometry_state_tracker.h"
+
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace kalman_filter {
+
+OdometryState::OdometryState(const common::Time time,
+                             const transform::Rigid3d& odometer_pose,
+                             const transform::Rigid3d& state_pose)
+    : time(time), odometer_pose(odometer_pose), state_pose(state_pose) {}
+
+OdometryStateTracker::OdometryStateTracker(const int window_size)
+    : window_size_(window_size) {
+  CHECK_GT(window_size, 0);
+}
+
+const OdometryStateTracker::OdometryStates&
+OdometryStateTracker::odometry_states() const {
+  return odometry_states_;
+}
+
+void OdometryStateTracker::AddOdometryState(
+    const OdometryState& odometry_state) {
+  odometry_states_.push_back(odometry_state);
+  while (odometry_states_.size() > window_size_) {
+    odometry_states_.pop_front();
+  }
+}
+
+bool OdometryStateTracker::empty() const { return odometry_states_.empty(); }
+
+const OdometryState& OdometryStateTracker::newest() const {
+  return odometry_states_.back();
+}
+
+}  // namespace kalman_filter
+}  // namespace cartographer

cartographer/cartographer/kalman_filter/odometry_state_tracker.h

@@ -0,0 +1,66 @@
+/*
+ * 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_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
+#define CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
+
+#include <deque>
+
+#include "cartographer/common/time.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace kalman_filter {
+
+struct OdometryState {
+  OdometryState(common::Time time, const transform::Rigid3d& odometer_pose,
+                const transform::Rigid3d& state_pose);
+  OdometryState() {}
+
+  common::Time time = common::Time::min();
+  transform::Rigid3d odometer_pose = transform::Rigid3d::Identity();
+  transform::Rigid3d state_pose = transform::Rigid3d::Identity();
+};
+
+// Keeps track of the odometry states by keeping sliding window over some
+// number of them.
+class OdometryStateTracker {
+ public:
+  using OdometryStates = std::deque<OdometryState>;
+
+  explicit OdometryStateTracker(int window_size);
+
+  const OdometryStates& odometry_states() const;
+
+  // Adds a new 'odometry_state' and makes sure the maximum number of previous
+  // odometry states is not exceeded.
+  void AddOdometryState(const OdometryState& odometry_state);
+
+  // Returns true if no elements are present in the odometry queue.
+  bool empty() const;
+
+  // Retrieves the most recent OdometryState or an empty one if non yet present.
+  const OdometryState& newest() const;
+
+ private:
+  OdometryStates odometry_states_;
+  size_t window_size_;
+};
+
+}  // namespace kalman_filter
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_

cartographer/cartographer/kalman_filter/pose_tracker.cc

@@ -0,0 +1,409 @@
+/*
+ * 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/kalman_filter/pose_tracker.h"
+
+#include <cmath>
+#include <limits>
+#include <utility>
+
+#include "Eigen/Geometry"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/math.h"
+#include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/gaussian_distribution.h"
+#include "cartographer/kalman_filter/unscented_kalman_filter.h"
+#include "cartographer/transform/transform.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace kalman_filter {
+
+namespace {
+
+PoseTracker::State AddDelta(const PoseTracker::State& state,
+                            const PoseTracker::State& delta) {
+  PoseTracker::State new_state = state + delta;
+  const Eigen::Quaterniond orientation =
+      transform::AngleAxisVectorToRotationQuaternion(
+          Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
+                          state[PoseTracker::kMapOrientationY],
+                          state[PoseTracker::kMapOrientationZ]));
+  const Eigen::Vector3d rotation_vector(delta[PoseTracker::kMapOrientationX],
+                                        delta[PoseTracker::kMapOrientationY],
+                                        delta[PoseTracker::kMapOrientationZ]);
+  CHECK_LT(rotation_vector.norm(), M_PI / 2.)
+      << "Sigma point is far from the mean, recovered delta may be incorrect.";
+  const Eigen::Quaterniond rotation =
+      transform::AngleAxisVectorToRotationQuaternion(rotation_vector);
+  const Eigen::Vector3d new_orientation =
+      transform::RotationQuaternionToAngleAxisVector(orientation * rotation);
+  new_state[PoseTracker::kMapOrientationX] = new_orientation.x();
+  new_state[PoseTracker::kMapOrientationY] = new_orientation.y();
+  new_state[PoseTracker::kMapOrientationZ] = new_orientation.z();
+  return new_state;
+}
+
+PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
+                                const PoseTracker::State& target) {
+  PoseTracker::State delta = target - origin;
+  const Eigen::Quaterniond origin_orientation =
+      transform::AngleAxisVectorToRotationQuaternion(
+          Eigen::Vector3d(origin[PoseTracker::kMapOrientationX],
+                          origin[PoseTracker::kMapOrientationY],
+                          origin[PoseTracker::kMapOrientationZ]));
+  const Eigen::Quaterniond target_orientation =
+      transform::AngleAxisVectorToRotationQuaternion(
+          Eigen::Vector3d(target[PoseTracker::kMapOrientationX],
+                          target[PoseTracker::kMapOrientationY],
+                          target[PoseTracker::kMapOrientationZ]));
+  const Eigen::Vector3d rotation =
+      transform::RotationQuaternionToAngleAxisVector(
+          origin_orientation.inverse() * target_orientation);
+  delta[PoseTracker::kMapOrientationX] = rotation.x();
+  delta[PoseTracker::kMapOrientationY] = rotation.y();
+  delta[PoseTracker::kMapOrientationZ] = rotation.z();
+  return delta;
+}
+
+// Build a model matrix for the given time delta.
+PoseTracker::State ModelFunction3D(const PoseTracker::State& state,
+                                   const double delta_t) {
+  CHECK_GT(delta_t, 0.);
+
+  PoseTracker::State new_state;
+  new_state[PoseTracker::kMapPositionX] =
+      state[PoseTracker::kMapPositionX] +
+      delta_t * state[PoseTracker::kMapVelocityX];
+  new_state[PoseTracker::kMapPositionY] =
+      state[PoseTracker::kMapPositionY] +
+      delta_t * state[PoseTracker::kMapVelocityY];
+  new_state[PoseTracker::kMapPositionZ] =
+      state[PoseTracker::kMapPositionZ] +
+      delta_t * state[PoseTracker::kMapVelocityZ];
+
+  new_state[PoseTracker::kMapOrientationX] =
+      state[PoseTracker::kMapOrientationX];
+  new_state[PoseTracker::kMapOrientationY] =
+      state[PoseTracker::kMapOrientationY];
+  new_state[PoseTracker::kMapOrientationZ] =
+      state[PoseTracker::kMapOrientationZ];
+
+  new_state[PoseTracker::kMapVelocityX] = state[PoseTracker::kMapVelocityX];
+  new_state[PoseTracker::kMapVelocityY] = state[PoseTracker::kMapVelocityY];
+  new_state[PoseTracker::kMapVelocityZ] = state[PoseTracker::kMapVelocityZ];
+
+  return new_state;
+}
+
+// A specialization of ModelFunction3D that limits the z-component of position
+// and velocity to 0.
+PoseTracker::State ModelFunction2D(const PoseTracker::State& state,
+                                   const double delta_t) {
+  auto new_state = ModelFunction3D(state, delta_t);
+  new_state[PoseTracker::kMapPositionZ] = 0.;
+  new_state[PoseTracker::kMapVelocityZ] = 0.;
+  new_state[PoseTracker::kMapOrientationX] = 0.;
+  new_state[PoseTracker::kMapOrientationY] = 0.;
+  return new_state;
+}
+
+}  // namespace
+
+ImuTracker::ImuTracker(const proto::PoseTrackerOptions& options,
+                       const common::Time time)
+    : options_(options),
+      time_(time),
+      last_linear_acceleration_time_(common::Time::min()),
+      orientation_(Eigen::Quaterniond::Identity()),
+      gravity_direction_(Eigen::Vector3d::UnitZ()),
+      imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
+
+void ImuTracker::Predict(const common::Time time) {
+  CHECK_LE(time_, time);
+  const double delta_t = common::ToSeconds(time - time_);
+  const Eigen::Quaterniond rotation =
+      transform::AngleAxisVectorToRotationQuaternion(
+          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
+  orientation_ = (orientation_ * rotation).normalized();
+  gravity_direction_ = rotation.inverse() * gravity_direction_;
+  time_ = time;
+}
+
+void ImuTracker::AddImuLinearAccelerationObservation(
+    const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
+  Predict(time);
+  // Update the 'gravity_direction_' with an exponential moving average using
+  // the 'imu_gravity_time_constant'.
+  const double delta_t =
+      last_linear_acceleration_time_ > common::Time::min()
+          ? common::ToSeconds(time - last_linear_acceleration_time_)
+          : std::numeric_limits<double>::infinity();
+  last_linear_acceleration_time_ = time;
+  const double alpha =
+      1. - std::exp(-delta_t / options_.imu_gravity_time_constant());
+  gravity_direction_ =
+      (1. - alpha) * gravity_direction_ + alpha * imu_linear_acceleration;
+  // Change the 'orientation_' so that it agrees with the current
+  // 'gravity_direction_'.
+  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
+      gravity_direction_, orientation_.inverse() * Eigen::Vector3d::UnitZ());
+  orientation_ = (orientation_ * rotation).normalized();
+  CHECK_GT((orientation_.inverse() * Eigen::Vector3d::UnitZ())
+               .dot(gravity_direction_),
+           0.);
+}
+
+void ImuTracker::AddImuAngularVelocityObservation(
+    const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
+  Predict(time);
+  imu_angular_velocity_ = imu_angular_velocity;
+}
+
+PoseAndCovariance operator*(const transform::Rigid3d& transform,
+                            const PoseAndCovariance& pose_and_covariance) {
+  GaussianDistribution<double, 6> distribution(
+      Eigen::Matrix<double, 6, 1>::Zero(), pose_and_covariance.covariance);
+  Eigen::Matrix<double, 6, 6> linear_transform;
+  linear_transform << transform.rotation().matrix(), Eigen::Matrix3d::Zero(),
+      Eigen::Matrix3d::Zero(), transform.rotation().matrix();
+  return {transform * pose_and_covariance.pose,
+          (linear_transform * distribution).GetCovariance()};
+}
+
+proto::PoseTrackerOptions CreatePoseTrackerOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::PoseTrackerOptions options;
+  options.set_position_model_variance(
+      parameter_dictionary->GetDouble("position_model_variance"));
+  options.set_orientation_model_variance(
+      parameter_dictionary->GetDouble("orientation_model_variance"));
+  options.set_velocity_model_variance(
+      parameter_dictionary->GetDouble("velocity_model_variance"));
+  options.set_imu_gravity_time_constant(
+      parameter_dictionary->GetDouble("imu_gravity_time_constant"));
+  options.set_imu_gravity_variance(
+      parameter_dictionary->GetDouble("imu_gravity_variance"));
+  options.set_num_odometry_states(
+      parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
+  CHECK_GT(options.num_odometry_states(), 0);
+  return options;
+}
+
+PoseTracker::Distribution PoseTracker::KalmanFilterInit() {
+  State initial_state = State::Zero();
+  // We are certain about the complete state at the beginning. We define the
+  // initial pose to be at the origin and axis aligned. Additionally, we claim
+  // that we are not moving.
+  StateCovariance initial_covariance = 1e-9 * StateCovariance::Identity();
+  return Distribution(initial_state, initial_covariance);
+}
+
+PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
+                         const ModelFunction& model_function,
+                         const common::Time time)
+    : options_(options),
+      model_function_(model_function),
+      time_(time),
+      kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
+      imu_tracker_(options, time),
+      odometry_state_tracker_(options.num_odometry_states()) {}
+
+PoseTracker::~PoseTracker() {}
+
+PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) {
+  Predict(time);
+  return kalman_filter_.GetBelief();
+}
+
+void PoseTracker::GetPoseEstimateMeanAndCovariance(const common::Time time,
+                                                   transform::Rigid3d* pose,
+                                                   PoseCovariance* covariance) {
+  const Distribution belief = GetBelief(time);
+  *pose = RigidFromState(belief.GetMean());
+  static_assert(kMapPositionX == 0, "Cannot extract PoseCovariance.");
+  static_assert(kMapPositionY == 1, "Cannot extract PoseCovariance.");
+  static_assert(kMapPositionZ == 2, "Cannot extract PoseCovariance.");
+  static_assert(kMapOrientationX == 3, "Cannot extract PoseCovariance.");
+  static_assert(kMapOrientationY == 4, "Cannot extract PoseCovariance.");
+  static_assert(kMapOrientationZ == 5, "Cannot extract PoseCovariance.");
+  *covariance = belief.GetCovariance().block<6, 6>(0, 0);
+  covariance->block<2, 2>(3, 3) +=
+      options_.imu_gravity_variance() * Eigen::Matrix2d::Identity();
+}
+
+const PoseTracker::Distribution PoseTracker::BuildModelNoise(
+    const double delta_t) const {
+  // Position is constant, but orientation changes.
+  StateCovariance model_noise = StateCovariance::Zero();
+
+  model_noise.diagonal() <<
+      // Position in map.
+      options_.position_model_variance() * delta_t,
+      options_.position_model_variance() * delta_t,
+      options_.position_model_variance() * delta_t,
+
+      // Orientation in map.
+      options_.orientation_model_variance() * delta_t,
+      options_.orientation_model_variance() * delta_t,
+      options_.orientation_model_variance() * delta_t,
+
+      // Linear velocities in map.
+      options_.velocity_model_variance() * delta_t,
+      options_.velocity_model_variance() * delta_t,
+      options_.velocity_model_variance() * delta_t;
+
+  return Distribution(State::Zero(), model_noise);
+}
+
+void PoseTracker::Predict(const common::Time time) {
+  imu_tracker_.Predict(time);
+  CHECK_LE(time_, time);
+  const double delta_t = common::ToSeconds(time - time_);
+  if (delta_t == 0.) {
+    return;
+  }
+  kalman_filter_.Predict(
+      [this, delta_t](const State& state) -> State {
+        switch (model_function_) {
+          case ModelFunction::k2D:
+            return ModelFunction2D(state, delta_t);
+          case ModelFunction::k3D:
+            return ModelFunction3D(state, delta_t);
+          default:
+            LOG(FATAL);
+        }
+      },
+      BuildModelNoise(delta_t));
+  time_ = time;
+}
+
+void PoseTracker::AddImuLinearAccelerationObservation(
+    const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
+  imu_tracker_.AddImuLinearAccelerationObservation(time,
+                                                   imu_linear_acceleration);
+  Predict(time);
+}
+
+void PoseTracker::AddImuAngularVelocityObservation(
+    const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
+  imu_tracker_.AddImuAngularVelocityObservation(time, imu_angular_velocity);
+  Predict(time);
+}
+
+void PoseTracker::AddPoseObservation(const common::Time time,
+                                     const transform::Rigid3d& pose,
+                                     const PoseCovariance& covariance) {
+  Predict(time);
+
+  // Noise covariance is taken directly from the input values.
+  const GaussianDistribution<double, 6> delta(
+      Eigen::Matrix<double, 6, 1>::Zero(), covariance);
+
+  kalman_filter_.Observe<6>(
+      [this, &pose](const State& state) -> Eigen::Matrix<double, 6, 1> {
+        const transform::Rigid3d state_pose = RigidFromState(state);
+        const Eigen::Vector3d delta_orientation =
+            transform::RotationQuaternionToAngleAxisVector(
+                pose.rotation().inverse() * state_pose.rotation());
+        const Eigen::Vector3d delta_translation =
+            state_pose.translation() - pose.translation();
+        Eigen::Matrix<double, 6, 1> return_value;
+        return_value << delta_translation, delta_orientation;
+        return return_value;
+      },
+      delta);
+}
+
+// Updates from the odometer are in the odometer's map-like frame, called the
+// 'odometry' frame. The odometer_pose converts data from the map frame
+// into the odometry frame.
+void PoseTracker::AddOdometerPoseObservation(
+    const common::Time time, const transform::Rigid3d& odometer_pose,
+    const PoseCovariance& covariance) {
+  if (!odometry_state_tracker_.empty()) {
+    const auto& previous_odometry_state = odometry_state_tracker_.newest();
+    const transform::Rigid3d delta =
+        previous_odometry_state.odometer_pose.inverse() * odometer_pose;
+    const transform::Rigid3d new_pose =
+        previous_odometry_state.state_pose * delta;
+    AddPoseObservation(time, new_pose, covariance);
+  }
+
+  const Distribution belief = GetBelief(time);
+
+  odometry_state_tracker_.AddOdometryState(
+      {time, odometer_pose, RigidFromState(belief.GetMean())});
+}
+
+const OdometryStateTracker::OdometryStates& PoseTracker::odometry_states()
+    const {
+  return odometry_state_tracker_.odometry_states();
+}
+
+transform::Rigid3d PoseTracker::RigidFromState(
+    const PoseTracker::State& state) {
+  return transform::Rigid3d(
+      Eigen::Vector3d(state[PoseTracker::kMapPositionX],
+                      state[PoseTracker::kMapPositionY],
+                      state[PoseTracker::kMapPositionZ]),
+      transform::AngleAxisVectorToRotationQuaternion(
+          Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
+                          state[PoseTracker::kMapOrientationY],
+                          state[PoseTracker::kMapOrientationZ])) *
+          imu_tracker_.orientation());
+}
+
+Pose2DCovariance Project2D(const PoseCovariance& covariance) {
+  Pose2DCovariance projected_covariance;
+  projected_covariance.block<2, 2>(0, 0) = covariance.block<2, 2>(0, 0);
+  projected_covariance.block<2, 1>(0, 2) = covariance.block<2, 1>(0, 5);
+  projected_covariance.block<1, 2>(2, 0) = covariance.block<1, 2>(5, 0);
+  projected_covariance(2, 2) = covariance(5, 5);
+  return projected_covariance;
+}
+
+PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
+                       const double position_variance,
+                       const double orientation_variance) {
+  PoseCovariance covariance;
+  covariance.setZero();
+  covariance.block<2, 2>(0, 0) = embedded_covariance.block<2, 2>(0, 0);
+  covariance.block<2, 1>(0, 5) = embedded_covariance.block<2, 1>(0, 2);
+  covariance.block<1, 2>(5, 0) = embedded_covariance.block<1, 2>(2, 0);
+  covariance(5, 5) = embedded_covariance(2, 2);
+
+  covariance(2, 2) = position_variance;
+  covariance(3, 3) = orientation_variance;
+  covariance(4, 4) = orientation_variance;
+  return covariance;
+}
+
+PoseCovariance PoseCovarianceFromProtoMatrix(
+    const sensor::proto::Matrix& proto_matrix) {
+  PoseCovariance covariance;
+  CHECK_EQ(proto_matrix.rows(), 6);
+  CHECK_EQ(proto_matrix.cols(), 6);
+  for (int i = 0; i < 6; ++i) {
+    for (int j = 0; j < 6; ++j) {
+      covariance(i, j) = proto_matrix.data(i * 6 + j);
+    }
+  }
+  return covariance;
+}
+
+}  // namespace kalman_filter
+}  // namespace cartographer

cartographer/cartographer/kalman_filter/pose_tracker.h

@@ -0,0 +1,176 @@
+/*
+ * 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_KALMAN_FILTER_POSE_TRACKER_H_
+#define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
+
+#include <deque>
+#include <memory>
+
+#include "Eigen/Cholesky"
+#include "Eigen/Core"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/gaussian_distribution.h"
+#include "cartographer/kalman_filter/odometry_state_tracker.h"
+#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
+#include "cartographer/kalman_filter/unscented_kalman_filter.h"
+#include "cartographer/sensor/proto/sensor.pb.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace kalman_filter {
+
+typedef Eigen::Matrix3d Pose2DCovariance;
+typedef Eigen::Matrix<double, 6, 6> PoseCovariance;
+
+struct PoseAndCovariance {
+  transform::Rigid3d pose;
+  PoseCovariance covariance;
+};
+
+PoseAndCovariance operator*(const transform::Rigid3d& transform,
+                            const PoseAndCovariance& pose_and_covariance);
+
+// Projects a 3D pose covariance into a 2D pose covariance.
+Pose2DCovariance Project2D(const PoseCovariance& embedded_covariance);
+
+// Embeds a 2D pose covariance into a 3D pose covariance.
+PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
+                       double position_variance, double orientation_variance);
+
+// Deserializes the 'proto_matrix' into a PoseCovariance.
+PoseCovariance PoseCovarianceFromProtoMatrix(
+    const sensor::proto::Matrix& proto_matrix);
+
+proto::PoseTrackerOptions CreatePoseTrackerOptions(
+    common::LuaParameterDictionary* parameter_dictionary);
+
+// Keeps track of the orientation using angular velocities and linear
+// accelerations from an IMU. Because averaged linear acceleration (assuming
+// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
+// though yaw does.
+class ImuTracker {
+ public:
+  ImuTracker(const proto::PoseTrackerOptions& options, common::Time time);
+
+  // Updates the orientation to reflect the given 'time'.
+  void Predict(common::Time time);
+
+  // Updates from an IMU reading (in the IMU frame).
+  void AddImuLinearAccelerationObservation(
+      common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
+  void AddImuAngularVelocityObservation(
+      common::Time time, const Eigen::Vector3d& imu_angular_velocity);
+
+  // Query the current orientation estimate.
+  Eigen::Quaterniond orientation() { return orientation_; }
+
+ private:
+  const proto::PoseTrackerOptions options_;
+  common::Time time_;
+  common::Time last_linear_acceleration_time_;
+  Eigen::Quaterniond orientation_;
+  Eigen::Vector3d gravity_direction_;
+  Eigen::Vector3d imu_angular_velocity_;
+};
+
+// A Kalman filter for a 3D state of position and orientation.
+// Includes functions to update from IMU and laser scan matches.
+class PoseTracker {
+ public:
+  enum {
+    kMapPositionX = 0,
+    kMapPositionY,
+    kMapPositionZ,
+    kMapOrientationX,
+    kMapOrientationY,
+    kMapOrientationZ,
+    kMapVelocityX,
+    kMapVelocityY,
+    kMapVelocityZ,
+    kDimension  // We terminate loops with this.
+  };
+
+  enum class ModelFunction { k2D, k3D };
+
+  using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;
+  using State = KalmanFilter::StateType;
+  using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;
+  using Distribution = GaussianDistribution<double, kDimension>;
+
+  // Create a new Kalman filter starting at the origin with a standard normal
+  // distribution at 'time'.
+  PoseTracker(const proto::PoseTrackerOptions& options,
+              const ModelFunction& model_function, common::Time time);
+  virtual ~PoseTracker();
+
+  // Sets 'pose' and 'covariance' to the mean and covariance of the belief at
+  // 'time' which must be >= to the current time. Must not be nullptr.
+  void GetPoseEstimateMeanAndCovariance(common::Time time,
+                                        transform::Rigid3d* pose,
+                                        PoseCovariance* covariance);
+
+  // Updates from an IMU reading (in the IMU frame).
+  void AddImuLinearAccelerationObservation(
+      common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
+  void AddImuAngularVelocityObservation(
+      common::Time time, const Eigen::Vector3d& imu_angular_velocity);
+
+  // Updates from a pose estimate in the map frame.
+  void AddPoseObservation(common::Time time, const transform::Rigid3d& pose,
+                          const PoseCovariance& covariance);
+
+  // Updates from a pose estimate in the odometer's map-like frame.
+  void AddOdometerPoseObservation(common::Time time,
+                                  const transform::Rigid3d& pose,
+                                  const PoseCovariance& covariance);
+
+  common::Time time() const { return time_; }
+
+  // Returns the belief at the 'time' which must be >= to the current time.
+  Distribution GetBelief(common::Time time);
+
+  const OdometryStateTracker::OdometryStates& odometry_states() const;
+
+ private:
+  // Returns the distribution required to initialize the KalmanFilter.
+  static Distribution KalmanFilterInit();
+
+  // Build a model noise distribution (zero mean) for the given time delta.
+  const Distribution BuildModelNoise(double delta_t) const;
+
+  // Predict the state forward in time. This is a no-op if 'time' has not moved
+  // forward.
+  void Predict(common::Time time);
+
+  // Computes a pose combining the given 'state' with the 'imu_tracker_'
+  // orientation.
+  transform::Rigid3d RigidFromState(const PoseTracker::State& state);
+
+  const proto::PoseTrackerOptions options_;
+  const ModelFunction model_function_;
+  common::Time time_;
+  KalmanFilter kalman_filter_;
+  ImuTracker imu_tracker_;
+  OdometryStateTracker odometry_state_tracker_;
+};
+
+}  // namespace kalman_filter
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_

cartographer/cartographer/kalman_filter/pose_tracker_test.cc

@@ -0,0 +1,256 @@
+/*
+ * 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/kalman_filter/pose_tracker.h"
+
+#include <random>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/rigid_transform_test_helpers.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace kalman_filter {
+namespace {
+
+constexpr double kOdometerVariance = 1e-12;
+
+using transform::IsNearly;
+using transform::Rigid3d;
+using ::testing::Not;
+
+class PoseTrackerTest : public ::testing::Test {
+ protected:
+  PoseTrackerTest() {
+    auto parameter_dictionary = common::MakeDictionary(R"text(
+        return {
+            orientation_model_variance = 1e-8,
+            position_model_variance = 1e-8,
+            velocity_model_variance = 1e-8,
+            imu_gravity_time_constant = 100.,
+            imu_gravity_variance = 1e-9,
+            num_odometry_states = 1,
+        }
+        )text");
+    const proto::PoseTrackerOptions options =
+        CreatePoseTrackerOptions(parameter_dictionary.get());
+    pose_tracker_ = common::make_unique<PoseTracker>(
+        options, PoseTracker::ModelFunction::k3D, common::FromUniversal(1000));
+  }
+
+  std::unique_ptr<PoseTracker> pose_tracker_;
+};
+
+TEST(CovarianceTest, EmbedAndProjectCovariance) {
+  std::mt19937 prng(42);
+  std::uniform_real_distribution<float> distribution(-10.f, 10.f);
+  for (int i = 0; i < 100; ++i) {
+    Pose2DCovariance covariance;
+    for (int row = 0; row < 3; ++row) {
+      for (int column = 0; column < 3; ++column) {
+        covariance(row, column) = distribution(prng);
+      }
+    }
+    const PoseCovariance embedded_covariance =
+        Embed3D(covariance, distribution(prng), distribution(prng));
+    EXPECT_TRUE(
+        (Project2D(embedded_covariance).array() == covariance.array()).all());
+  }
+}
+
+TEST_F(PoseTrackerTest, SaveAndRestore) {
+  std::vector<Rigid3d> poses(3);
+  std::vector<PoseCovariance> covariances(3);
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500),
+                                                  &poses[0], &covariances[0]);
+
+  pose_tracker_->AddImuLinearAccelerationObservation(
+      common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
+
+  PoseTracker copy_of_pose_tracker = *pose_tracker_;
+
+  const Eigen::Vector3d observation(2, 0, 8);
+  pose_tracker_->AddImuLinearAccelerationObservation(
+      common::FromUniversal(3000), observation);
+
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(3500),
+                                                  &poses[1], &covariances[1]);
+
+  copy_of_pose_tracker.AddImuLinearAccelerationObservation(
+      common::FromUniversal(3000), observation);
+  copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance(
+      common::FromUniversal(3500), &poses[2], &covariances[2]);
+
+  EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
+  EXPECT_FALSE((covariances[0].array() == covariances[1].array()).all());
+  EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
+  EXPECT_TRUE((covariances[1].array() == covariances[2].array()).all());
+}
+
+TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
+  auto time = common::FromUniversal(1000);
+
+  for (int i = 0; i < 300; ++i) {
+    time += std::chrono::seconds(5);
+    pose_tracker_->AddImuLinearAccelerationObservation(
+        time, Eigen::Vector3d(0., 0., 10.));
+  }
+
+  {
+    Rigid3d pose;
+    PoseCovariance covariance;
+    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
+    const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
+    const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
+    EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
+                                                 << actual.coeffs();
+  }
+
+  for (int i = 0; i < 300; ++i) {
+    time += std::chrono::seconds(5);
+    pose_tracker_->AddImuLinearAccelerationObservation(
+        time, Eigen::Vector3d(0., 10., 0.));
+  }
+
+  time += std::chrono::milliseconds(5);
+
+  Rigid3d pose;
+  PoseCovariance covariance;
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
+  const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
+  const Eigen::Quaterniond expected = Eigen::Quaterniond(
+      Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
+  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
+                                               << actual.coeffs();
+}
+
+TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
+  auto time = common::FromUniversal(1000);
+
+  for (int i = 0; i < 300; ++i) {
+    time += std::chrono::milliseconds(5);
+    pose_tracker_->AddImuAngularVelocityObservation(time,
+                                                    Eigen::Vector3d::Zero());
+  }
+
+  {
+    Rigid3d pose;
+    PoseCovariance covariance;
+    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
+    const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
+    const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
+    EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
+                                                 << actual.coeffs();
+  }
+
+  const double target_radians = M_PI / 2.;
+  const double num_observations = 300.;
+  const double angular_velocity = target_radians / (num_observations * 5e-3);
+  for (int i = 0; i < num_observations; ++i) {
+    time += std::chrono::milliseconds(5);
+    pose_tracker_->AddImuAngularVelocityObservation(
+        time, Eigen::Vector3d(angular_velocity, 0., 0.));
+  }
+
+  time += std::chrono::milliseconds(5);
+
+  Rigid3d pose;
+  PoseCovariance covariance;
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
+  const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
+  const Eigen::Quaterniond expected = Eigen::Quaterniond(
+      Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
+  EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
+                                               << actual.coeffs();
+}
+
+TEST_F(PoseTrackerTest, AddPoseObservation) {
+  auto time = common::FromUniversal(1000);
+
+  for (int i = 0; i < 300; ++i) {
+    time += std::chrono::milliseconds(5);
+    pose_tracker_->AddPoseObservation(
+        time, Rigid3d::Identity(),
+        Eigen::Matrix<double, 6, 6>::Identity() * 1e-6);
+  }
+
+  {
+    Rigid3d actual;
+    PoseCovariance covariance;
+    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
+    EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
+  }
+
+  const Rigid3d expected =
+      Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
+      Rigid3d::Rotation(Eigen::AngleAxisd(
+          M_PI / 2., Eigen::Vector3d(0., 0., 3.).normalized()));
+
+  for (int i = 0; i < 300; ++i) {
+    time += std::chrono::milliseconds(15);
+    pose_tracker_->AddPoseObservation(
+        time, expected, Eigen::Matrix<double, 6, 6>::Identity() * 1e-9);
+  }
+
+  time += std::chrono::milliseconds(15);
+
+  Rigid3d actual;
+  PoseCovariance covariance;
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
+  EXPECT_THAT(actual, IsNearly(expected, 1e-3));
+}
+
+TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
+  common::Time time = common::FromUniversal(0);
+
+  std::vector<Rigid3d> odometer_track;
+  odometer_track.push_back(Rigid3d::Identity());
+  odometer_track.push_back(
+      Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
+  odometer_track.push_back(
+      Rigid3d::Translation(Eigen::Vector3d(0.2, 0., 0.)) *
+      Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
+  odometer_track.push_back(
+      Rigid3d::Translation(Eigen::Vector3d(0.3, 0.1, 0.)) *
+      Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
+  odometer_track.push_back(
+      Rigid3d::Translation(Eigen::Vector3d(0.2, 0.2, 0.1)) *
+      Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
+  odometer_track.push_back(
+      Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.2)) *
+      Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
+  odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
+
+  Rigid3d actual;
+  PoseCovariance unused_covariance;
+  for (const Rigid3d& pose : odometer_track) {
+    time += std::chrono::seconds(1);
+    pose_tracker_->AddOdometerPoseObservation(
+        time, pose, kOdometerVariance * PoseCovariance::Identity());
+    pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual,
+                                                    &unused_covariance);
+    EXPECT_THAT(actual, IsNearly(pose, 1e-2));
+  }
+  // Sanity check that the test has signal:
+  EXPECT_THAT(actual, Not(IsNearly(odometer_track[0], 1e-2)));
+}
+
+}  // namespace
+}  // namespace kalman_filter
+}  // namespace cartographer

cartographer/cartographer/kalman_filter/proto/CMakeLists.txt

@@ -0,0 +1,18 @@
+# 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.
+
+google_proto_library(kalman_filter_proto_pose_tracker_options
+  SRCS
+    pose_tracker_options.proto
+)

cartographer/cartographer/kalman_filter/proto/pose_tracker_options.proto

@@ -0,0 +1,32 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.kalman_filter.proto;
+
+message PoseTrackerOptions {
+  // Model variances depend linearly on time.
+  optional double position_model_variance = 1;
+  optional double orientation_model_variance = 2;
+  optional double velocity_model_variance = 3;
+
+  // Time constant for the orientation moving average based on observed gravity
+  // via linear acceleration.
+  optional double imu_gravity_time_constant = 4;
+  optional double imu_gravity_variance = 5;
+
+  // Maximum number of previous odometry states to keep.
+  optional int32 num_odometry_states = 6;
+}

cartographer/cartographer/kalman_filter/unscented_kalman_filter.h

@@ -0,0 +1,291 @@
+/*
+ * 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_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
+#define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <vector>
+
+#include "Eigen/Cholesky"
+#include "Eigen/Core"
+#include "Eigen/Eigenvalues"
+#include "cartographer/kalman_filter/gaussian_distribution.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace kalman_filter {
+
+template <typename FloatType>
+constexpr FloatType sqr(FloatType a) {
+  return a * a;
+}
+
+template <typename FloatType, int N>
+Eigen::Matrix<FloatType, N, N> OuterProduct(
+    const Eigen::Matrix<FloatType, N, 1>& v) {
+  return v * v.transpose();
+}
+
+// Checks if 'A' is a symmetric matrix.
+template <typename FloatType, int N>
+void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A) {
+  // This should be pretty much Eigen::Matrix<>::Zero() if the matrix is
+  // symmetric.
+  const FloatType norm = (A - A.transpose()).norm();
+  CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5)
+      << "Symmetry check failed with norm: '" << norm << "' from matrix:\n"
+      << A;
+}
+
+// Returns the matrix square root of a symmetric positive semidefinite matrix.
+template <typename FloatType, int N>
+Eigen::Matrix<FloatType, N, N> MatrixSqrt(
+    const Eigen::Matrix<FloatType, N, N>& A) {
+  CheckSymmetric(A);
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FloatType, N, N>>
+      adjoint_eigen_solver((A + A.transpose()) / 2.);
+  const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();
+  CHECK_GT(eigenvalues.minCoeff(), -1e-5)
+      << "MatrixSqrt failed with negative eigenvalues: "
+      << eigenvalues.transpose();
+
+  return adjoint_eigen_solver.eigenvectors() *
+         adjoint_eigen_solver.eigenvalues()
+             .cwiseMax(Eigen::Matrix<FloatType, N, 1>::Zero())
+             .cwiseSqrt()
+             .asDiagonal() *
+         adjoint_eigen_solver.eigenvectors().transpose();
+}
+
+// Implementation of a Kalman filter. We follow the nomenclature from
+// Thrun, S. et al., Probabilistic Robotics, 2006.
+//
+// Extended to handle non-additive noise/sensors inspired by Kraft, E., A
+// Quaternion-based Unscented Kalman Filter for Orientation Tracking.
+template <typename FloatType, int N>
+class UnscentedKalmanFilter {
+ public:
+  using StateType = Eigen::Matrix<FloatType, N, 1>;
+  using StateCovarianceType = Eigen::Matrix<FloatType, N, N>;
+
+  explicit UnscentedKalmanFilter(
+      const GaussianDistribution<FloatType, N>& initial_belief,
+      std::function<StateType(const StateType& state, const StateType& delta)>
+          add_delta = [](const StateType& state,
+                         const StateType& delta) { return state + delta; },
+      std::function<StateType(const StateType& origin, const StateType& target)>
+          compute_delta =
+              [](const StateType& origin, const StateType& target) {
+                return target - origin;
+              })
+      : belief_(initial_belief),
+        add_delta_(add_delta),
+        compute_delta_(compute_delta) {}
+
+  // Does the control/prediction step for the filter. The control must be
+  // implicitly added by the function g which also does the state transition.
+  // 'epsilon' is the additive combination of control and model noise.
+  void Predict(std::function<StateType(const StateType&)> g,
+               const GaussianDistribution<FloatType, N>& epsilon) {
+    CheckSymmetric(epsilon.GetCovariance());
+
+    // Get the state mean and matrix root of its covariance.
+    const StateType& mu = belief_.GetMean();
+    const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
+
+    std::vector<StateType> Y;
+    Y.reserve(2 * N + 1);
+    Y.emplace_back(g(mu));
+
+    const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
+    for (int i = 0; i < N; ++i) {
+      // Order does not matter here as all have the same weights in the
+      // summation later on anyways.
+      Y.emplace_back(g(add_delta_(mu, kSqrtNPlusLambda * sqrt_sigma.col(i))));
+      Y.emplace_back(g(add_delta_(mu, -kSqrtNPlusLambda * sqrt_sigma.col(i))));
+    }
+    const StateType new_mu = ComputeMean(Y);
+
+    StateCovarianceType new_sigma =
+        kCovWeight0 * OuterProduct<FloatType, N>(compute_delta_(new_mu, Y[0]));
+    for (int i = 0; i < N; ++i) {
+      new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
+                                     compute_delta_(new_mu, Y[2 * i + 1]));
+      new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
+                                     compute_delta_(new_mu, Y[2 * i + 2]));
+    }
+    CheckSymmetric(new_sigma);
+
+    belief_ = GaussianDistribution<FloatType, N>(new_mu, new_sigma) + epsilon;
+  }
+
+  // The observation step of the Kalman filter. 'h' transfers the state
+  // into an observation that should be zero, i.e., the sensor readings should
+  // be included in this function already. 'delta' is the measurement noise and
+  // must have zero mean.
+  template <int K>
+  void Observe(
+      std::function<Eigen::Matrix<FloatType, K, 1>(const StateType&)> h,
+      const GaussianDistribution<FloatType, K>& delta) {
+    CheckSymmetric(delta.GetCovariance());
+    // We expect zero mean delta.
+    CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9);
+
+    // Get the state mean and matrix root of its covariance.
+    const StateType& mu = belief_.GetMean();
+    const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
+
+    // As in Kraft's paper, we compute W containing the zero-mean sigma points,
+    // since this is all we need.
+    std::vector<StateType> W;
+    W.reserve(2 * N + 1);
+    W.emplace_back(StateType::Zero());
+
+    std::vector<Eigen::Matrix<FloatType, K, 1>> Z;
+    Z.reserve(2 * N + 1);
+    Z.emplace_back(h(mu));
+
+    Eigen::Matrix<FloatType, K, 1> z_hat = kMeanWeight0 * Z[0];
+    const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
+    for (int i = 0; i < N; ++i) {
+      // Order does not matter here as all have the same weights in the
+      // summation later on anyways.
+      W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i));
+      Z.emplace_back(h(add_delta_(mu, W.back())));
+
+      W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i));
+      Z.emplace_back(h(add_delta_(mu, W.back())));
+
+      z_hat += kMeanWeightI * Z[2 * i + 1];
+      z_hat += kMeanWeightI * Z[2 * i + 2];
+    }
+
+    Eigen::Matrix<FloatType, K, K> S =
+        kCovWeight0 * OuterProduct<FloatType, K>(Z[0] - z_hat);
+    for (int i = 0; i < N; ++i) {
+      S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 1] - z_hat);
+      S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 2] - z_hat);
+    }
+    CheckSymmetric(S);
+    S += delta.GetCovariance();
+
+    Eigen::Matrix<FloatType, N, K> sigma_bar_xz =
+        kCovWeight0 * W[0] * (Z[0] - z_hat).transpose();
+    for (int i = 0; i < N; ++i) {
+      sigma_bar_xz +=
+          kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose();
+      sigma_bar_xz +=
+          kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose();
+    }
+
+    const Eigen::Matrix<FloatType, N, K> kalman_gain =
+        sigma_bar_xz * S.inverse();
+    const StateCovarianceType new_sigma =
+        belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose();
+    CheckSymmetric(new_sigma);
+
+    belief_ = GaussianDistribution<FloatType, N>(
+        add_delta_(mu, kalman_gain * -z_hat), new_sigma);
+  }
+
+  const GaussianDistribution<FloatType, N>& GetBelief() const {
+    return belief_;
+  }
+
+ private:
+  StateType ComputeWeightedError(const StateType& mean_estimate,
+                                 const std::vector<StateType>& states) {
+    StateType weighted_error =
+        kMeanWeight0 * compute_delta_(mean_estimate, states[0]);
+    for (int i = 1; i != 2 * N + 1; ++i) {
+      weighted_error += kMeanWeightI * compute_delta_(mean_estimate, states[i]);
+    }
+    return weighted_error;
+  }
+
+  // Algorithm for computing the mean of non-additive states taken from Kraft's
+  // Section 3.4, adapted to our implementation.
+  StateType ComputeMean(const std::vector<StateType>& states) {
+    CHECK_EQ(states.size(), 2 * N + 1);
+    StateType current_estimate = states[0];
+    StateType weighted_error = ComputeWeightedError(current_estimate, states);
+    int iterations = 0;
+    while (weighted_error.norm() > 1e-9) {
+      double step_size = 1.;
+      while (true) {
+        const StateType next_estimate =
+            add_delta_(current_estimate, step_size * weighted_error);
+        const StateType next_error =
+            ComputeWeightedError(next_estimate, states);
+        if (next_error.norm() < weighted_error.norm()) {
+          current_estimate = next_estimate;
+          weighted_error = next_error;
+          break;
+        }
+        step_size *= 0.5;
+        CHECK_GT(step_size, 1e-3) << "Step size too small, line search failed.";
+      }
+      ++iterations;
+      CHECK_LT(iterations, 20) << "Too many iterations.";
+    }
+    return current_estimate;
+  }
+
+  // According to Wikipedia these are the normal values. Thrun does not
+  // mention those.
+  constexpr static FloatType kAlpha = 1e-3;
+  constexpr static FloatType kKappa = 0.;
+  constexpr static FloatType kBeta = 2.;
+  constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;
+  constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);
+  constexpr static FloatType kCovWeight0 =
+      kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);
+  constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));
+  constexpr static FloatType kCovWeightI = kMeanWeightI;
+
+  GaussianDistribution<FloatType, N> belief_;
+  const std::function<StateType(const StateType& state, const StateType& delta)>
+      add_delta_;
+  const std::function<StateType(const StateType& origin,
+                                const StateType& target)>
+      compute_delta_;
+};
+
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kAlpha;
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kKappa;
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kBeta;
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kLambda;
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeight0;
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeight0;
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeightI;
+template <typename FloatType, int N>
+constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeightI;
+
+}  // namespace kalman_filter
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_

cartographer/cartographer/kalman_filter/unscented_kalman_filter_test.cc

@@ -0,0 +1,74 @@
+/*
+ * 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/kalman_filter/unscented_kalman_filter.h"
+
+#include "cartographer/kalman_filter/gaussian_distribution.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace kalman_filter {
+namespace {
+
+Eigen::Matrix<double, 1, 1> Scalar(double value) {
+  return value * Eigen::Matrix<double, 1, 1>::Identity();
+}
+
+// A simple model that copies the first to the second state variable.
+Eigen::Matrix<double, 2, 1> g(const Eigen::Matrix<double, 2, 1>& state) {
+  Eigen::Matrix<double, 2, 1> new_state;
+  new_state << state[0], state[0];
+  return new_state;
+}
+
+// A simple observation of the first state variable.
+Eigen::Matrix<double, 1, 1> h(const Eigen::Matrix<double, 2, 1>& state) {
+  return Scalar(state[0]) - Scalar(5.);
+}
+
+TEST(KalmanFilterTest, testConstructor) {
+  UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
+      Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
+  EXPECT_NEAR(42., filter.GetBelief().GetMean()[1], 1e-9);
+}
+
+TEST(KalmanFilterTest, testPredict) {
+  UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
+      Eigen::Vector2d(42., 0.), 10. * Eigen::Matrix2d::Identity()));
+  filter.Predict(
+      g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
+                                         Eigen::Matrix2d::Identity() * 1e-9));
+  EXPECT_NEAR(filter.GetBelief().GetMean()[0], 42., 1e-2);
+  EXPECT_NEAR(filter.GetBelief().GetMean()[1], 42., 1e-2);
+}
+
+TEST(KalmanFilterTest, testObserve) {
+  UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
+      Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
+  for (int i = 0; i < 500; ++i) {
+    filter.Predict(
+        g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
+                                           Eigen::Matrix2d::Identity() * 1e-9));
+    filter.Observe<1>(
+        h, GaussianDistribution<double, 1>(Scalar(0.), Scalar(1e-2)));
+  }
+  EXPECT_NEAR(filter.GetBelief().GetMean()[0], 5, 1e-2);
+  EXPECT_NEAR(filter.GetBelief().GetMean()[1], 5, 1e-2);
+}
+
+}  // namespace
+}  // namespace kalman_filter
+}  // namespace cartographer

cartographer/cartographer/mapping/CMakeLists.txt

@@ -0,0 +1,150 @@
+# 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.
+
+add_subdirectory("proto")
+add_subdirectory("sparse_pose_graph")
+
+google_library(mapping_global_trajectory_builder_interface
+  SRCS
+    global_trajectory_builder_interface.cc
+  HDRS
+    global_trajectory_builder_interface.h
+  DEPENDS
+    common_time
+    kalman_filter_pose_tracker
+    mapping_submaps
+    sensor_laser
+    sensor_point_cloud
+)
+
+google_library(mapping_probability_values
+  USES_CERES
+  SRCS
+    probability_values.cc
+  HDRS
+    probability_values.h
+  DEPENDS
+    common_math
+    common_port
+)
+
+google_library(mapping_sensor_collator
+  USES_CERES
+  USES_EIGEN
+  HDRS
+    sensor_collator.h
+  DEPENDS
+    common_make_unique
+    common_ordered_multi_queue
+    common_time
+    sensor_sensor_packet_period_histogram_builder
+)
+
+google_library(mapping_sparse_pose_graph
+  USES_CERES
+  SRCS
+    sparse_pose_graph.cc
+  HDRS
+    sparse_pose_graph.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    mapping_proto_scan_matching_progress
+    mapping_proto_sparse_pose_graph_options
+    mapping_sparse_pose_graph_constraint_builder
+    mapping_sparse_pose_graph_optimization_problem_options
+    mapping_submaps
+    mapping_trajectory_node
+    transform_rigid_transform
+)
+
+google_library(mapping_submaps
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    submaps.cc
+  HDRS
+    submaps.h
+  DEPENDS
+    common_math
+    common_port
+    mapping_2d_probability_grid
+    mapping_probability_values
+    mapping_proto_submaps
+    mapping_trajectory_node
+    transform_transform
+)
+
+google_library(mapping_trajectory_connectivity
+  USES_CERES
+  SRCS
+    trajectory_connectivity.cc
+  HDRS
+    trajectory_connectivity.h
+  DEPENDS
+    common_mutex
+    mapping_proto_trajectory_connectivity
+    mapping_submaps
+)
+
+google_library(mapping_trajectory_node
+  USES_EIGEN
+  HDRS
+    trajectory_node.h
+  DEPENDS
+    common_time
+    sensor_laser
+    transform_rigid_transform
+)
+
+google_test(mapping_probability_values_test
+  SRCS
+    probability_values_test.cc
+  DEPENDS
+    mapping_probability_values
+)
+
+google_test(mapping_sensor_collator_test
+  SRCS
+    sensor_collator_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary_test_helpers
+    common_make_unique
+    common_time
+    mapping_sensor_collator
+    sensor_proto_sensor
+)
+
+google_test(mapping_sparse_pose_graph_test
+  USES_CERES
+  SRCS
+    sparse_pose_graph_test.cc
+  DEPENDS
+    mapping_sparse_pose_graph
+)
+
+google_test(mapping_submaps_test
+  SRCS
+    submaps_test.cc
+  DEPENDS
+    mapping_submaps
+)
+
+google_test(mapping_trajectory_connectivity_test
+  SRCS
+    trajectory_connectivity_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary_test_helpers
+    mapping_2d_submaps
+    mapping_trajectory_connectivity
+)

cartographer/cartographer/mapping/global_trajectory_builder_interface.cc

@@ -0,0 +1,35 @@
+/*
+ * 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/cartographer/mapping/global_trajectory_builder_interface.h

@@ -0,0 +1,97 @@
+/*
+ * 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_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
+#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/pose_tracker.h"
+#include "cartographer/mapping/submaps.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. Implementations wire up a
+// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
+// to detect loop closure, and a sparse pose graph optimization to compute
+// optimized pose estimates.
+class GlobalTrajectoryBuilderInterface {
+ 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 odometry frame, used to make
+  // this estimate.
+  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() {}
+
+  GlobalTrajectoryBuilderInterface(const GlobalTrajectoryBuilderInterface&) =
+      delete;
+  GlobalTrajectoryBuilderInterface& operator=(
+      const GlobalTrajectoryBuilderInterface&) = 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 AddHorizontalLaserFan(common::Time,
+                                     const sensor::LaserFan3D& laser_fan) = 0;
+  virtual void AddImuData(common::Time time,
+                          const Eigen::Vector3d& linear_acceleration,
+                          const Eigen::Vector3d& angular_velocity) = 0;
+  virtual void AddLaserFan3D(common::Time time,
+                             const sensor::LaserFan3D& laser_fan) = 0;
+  virtual void AddOdometerPose(
+      common::Time time, const transform::Rigid3d& pose,
+      const kalman_filter::PoseCovariance& covariance) = 0;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_

cartographer/cartographer/mapping/probability_values.cc

@@ -0,0 +1,66 @@
+/*
+ * 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/probability_values.h"
+
+namespace cartographer {
+namespace mapping {
+
+namespace {
+
+// 0 is unknown, [1, 32767] maps to [kMinProbability, kMaxProbability].
+float SlowValueToProbability(const uint16 value) {
+  CHECK_GE(value, 0);
+  CHECK_LE(value, 32767);
+  if (value == kUnknownProbabilityValue) {
+    // Unknown cells have kMinProbability.
+    return kMinProbability;
+  }
+  const float kScale = (kMaxProbability - kMinProbability) / 32766.f;
+  return value * kScale + (kMinProbability - kScale);
+}
+
+const std::vector<float>* PrecomputeValueToProbability() {
+  std::vector<float>* result = new std::vector<float>;
+  // Repeat two times, so that both values with and without the update marker
+  // can be converted to a probability.
+  for (int repeat = 0; repeat != 2; ++repeat) {
+    for (int value = 0; value != 32768; ++value) {
+      result->push_back(SlowValueToProbability(value));
+    }
+  }
+  return result;
+}
+
+}  // namespace
+
+const std::vector<float>* const kValueToProbability =
+    PrecomputeValueToProbability();
+
+std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
+  std::vector<uint16> result;
+  result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) +
+                   kUpdateMarker);
+  for (int cell = 1; cell != 32768; ++cell) {
+    result.push_back(ProbabilityToValue(ProbabilityFromOdds(
+                         odds * Odds((*kValueToProbability)[cell]))) +
+                     kUpdateMarker);
+  }
+  return result;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/probability_values.h

@@ -0,0 +1,74 @@
+/*
+ * 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_PROBABILITY_VALUES_H_
+#define CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_
+
+#include <cmath>
+#include <vector>
+
+#include "cartographer/common/math.h"
+#include "cartographer/common/port.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+inline float Odds(float probability) {
+  return probability / (1.f - probability);
+}
+
+inline float ProbabilityFromOdds(const float odds) {
+  return odds / (odds + 1.f);
+}
+
+constexpr float kMinProbability = 0.1f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+
+// Clamps probability to be in the range [kMinProbability, kMaxProbability].
+inline float ClampProbability(const float probability) {
+  return common::Clamp(probability, kMinProbability, kMaxProbability);
+}
+
+constexpr uint16 kUnknownProbabilityValue = 0;
+constexpr uint16 kUpdateMarker = 1u << 15;
+
+// Converts a probability to a uint16 in the [1, 32767] range.
+inline uint16 ProbabilityToValue(const float probability) {
+  const int value =
+      common::RoundToInt((ClampProbability(probability) - kMinProbability) *
+                         (32766.f / (kMaxProbability - kMinProbability))) +
+      1;
+  // DCHECK for performance.
+  DCHECK_GE(value, 1);
+  DCHECK_LE(value, 32767);
+  return value;
+}
+
+extern const std::vector<float>* const kValueToProbability;
+
+// Converts a uint16 (which may or may not have the update marker set) to a
+// probability in the range [kMinProbability, kMaxProbability].
+inline float ValueToProbability(const uint16 value) {
+  return (*kValueToProbability)[value];
+}
+
+std::vector<uint16> ComputeLookupTableToApplyOdds(float odds);
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_

cartographer/cartographer/mapping/probability_values_test.cc

@@ -0,0 +1,34 @@
+/*
+ * 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/probability_values.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+TEST(ProbabilityValuesTest, OddsConversions) {
+  EXPECT_NEAR(ProbabilityFromOdds(Odds(kMinProbability)), kMinProbability,
+              1e-6);
+  EXPECT_NEAR(ProbabilityFromOdds(Odds(kMaxProbability)), kMaxProbability,
+              1e-6);
+  EXPECT_NEAR(ProbabilityFromOdds(Odds(0.5)), 0.5, 1e-6);
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/proto/CMakeLists.txt

@@ -0,0 +1,38 @@
+# 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.
+
+google_proto_library(mapping_proto_scan_matching_progress
+  SRCS
+    scan_matching_progress.proto
+)
+
+google_proto_library(mapping_proto_sparse_pose_graph_options
+  SRCS
+    sparse_pose_graph_options.proto
+  DEPENDS
+    mapping_sparse_pose_graph_proto_constraint_builder_options
+    mapping_sparse_pose_graph_proto_optimization_problem_options
+)
+
+google_proto_library(mapping_proto_submaps
+  SRCS
+    submaps.proto
+  DEPENDS
+    transform_proto_transform
+)
+
+google_proto_library(mapping_proto_trajectory_connectivity
+  SRCS
+    trajectory_connectivity.proto
+)

cartographer/cartographer/mapping/proto/scan_matching_progress.proto

@@ -0,0 +1,26 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping.proto;
+
+// This is how proto2 calls the outer class since there is already a message
+// with the same name in the file.
+option java_outer_classname = "ScanMatchingProgressOuterClass";
+
+message ScanMatchingProgress {
+  optional int64 num_scans_finished = 1;
+  optional int64 num_scans_total = 2;
+}

cartographer/cartographer/mapping/proto/sparse_pose_graph_options.proto

@@ -0,0 +1,45 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping.proto;
+
+import "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto";
+import "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto";
+
+message SparsePoseGraphOptions {
+  // Online loop closure: If positive, will run the loop closure while the map
+  // is built.
+  optional int32 optimize_every_n_scans = 1;
+
+  // If true, constraints between old scans and new submaps will be computed.
+  optional bool also_match_to_new_submaps = 2;
+
+  // Options for the constraint builder.
+  optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
+      constraint_builder_options = 3;
+
+  // Options for the optimization problem.
+  optional mapping.sparse_pose_graph.proto.OptimizationProblemOptions
+      optimization_problem_options = 4;
+
+  // Number of iterations to use in 'optimization_problem_options' for the final
+  // optimization.
+  optional int32 max_num_final_iterations = 6;
+
+  // Rate at which we sample a single trajectory's scans for global
+  // localization.
+  optional double global_sampling_ratio = 5;
+};

cartographer/cartographer/mapping/proto/submaps.proto

@@ -0,0 +1,70 @@
+// 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.
+
+syntax = "proto2";
+
+import "cartographer/transform/proto/transform.proto";
+
+package cartographer.mapping.proto;
+
+message SubmapList {
+  message SubmapEntry {
+    optional int32 submap_version = 1;
+    optional transform.proto.Rigid3d pose = 3;
+  }
+
+  message TrajectorySubmapList {
+    repeated SubmapEntry submap = 1;
+  }
+
+  repeated TrajectorySubmapList trajectory = 2;
+}
+
+message SubmapQuery {
+  message Request {
+    // Index into 'SubmapList.trajectory(trajectory_id).submap'.
+    optional int32 submap_id = 1;
+    // Index into 'TrajectoryList.trajectory'.
+    optional int32 trajectory_id = 2;
+  }
+
+  message Response {
+    // ID of the Submap.
+    optional int32 submap_id = 1;
+
+    // Version of the given submap, higher means newer.
+    optional int32 submap_version = 2;
+
+    // GZipped map data, in row-major order, starting with (0,0). Each cell
+    // consists of two bytes: value (premultiplied by alpha) and alpha.
+    optional bytes cells = 3;
+
+    // Dimensions of the grid in cells.
+    optional int32 width = 4;
+    optional int32 height = 5;
+
+    // Size of one cell in meters.
+    optional double resolution = 6;
+
+    // Pose of the resolution*width x resolution*height rectangle in the submap
+    // frame.
+    optional transform.proto.Rigid3d slice_pose = 9;
+
+    // Error message in response to malformed requests.
+    optional string error_message = 8;
+  }
+
+  optional Request request = 1;
+  optional Response response = 2;
+}

cartographer/cartographer/mapping/proto/trajectory_connectivity.proto

@@ -0,0 +1,30 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping.proto;
+
+// This is how proto2 calls the outer class since there is already a message
+// with the same name in the file.
+option java_outer_classname = "TrajectoryConnectivityOuterClass";
+
+// Connectivity structure between trajectories.
+message TrajectoryConnectivity {
+  message ConnectedComponent {
+    repeated int32 trajectory_id = 1;
+  }
+
+  repeated ConnectedComponent connected_component = 1;
+}

cartographer/cartographer/mapping/sensor_collator.h

@@ -0,0 +1,131 @@
+/*
+ * 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_SENSOR_COLLATOR_H_
+#define CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_
+
+#include <functional>
+#include <memory>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/ordered_multi_queue.h"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/sensor_packet_period_histogram_builder.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+struct SensorCollatorQueueKey {
+  int trajectory_id;
+  string sensor_id;
+
+  bool operator<(const SensorCollatorQueueKey& other) const {
+    return std::forward_as_tuple(trajectory_id, sensor_id) <
+           std::forward_as_tuple(other.trajectory_id, other.sensor_id);
+  }
+};
+
+inline std::ostream& operator<<(std::ostream& out,
+                                const SensorCollatorQueueKey& key) {
+  return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')';
+}
+
+template <typename SensorDataType>
+class SensorCollator {
+ public:
+  using Callback = std::function<void(int64, std::unique_ptr<SensorDataType>)>;
+
+  SensorCollator() {}
+
+  SensorCollator(const SensorCollator&) = delete;
+  SensorCollator& operator=(const SensorCollator&) = delete;
+
+  // Adds a trajectory to produce sorted sensor output for. Calls 'callback'
+  // for each collated sensor data.
+  void AddTrajectory(const int trajectory_id,
+                     const std::unordered_set<string>& expected_sensor_ids,
+                     const Callback callback) {
+    for (const auto& sensor_id : expected_sensor_ids) {
+      const auto queue_key = SensorCollatorQueueKey{trajectory_id, sensor_id};
+      queue_.AddQueue(queue_key, [callback](std::unique_ptr<Value> value) {
+        callback(value->timestamp, std::move(value->sensor_data));
+      });
+      queue_keys_[trajectory_id].push_back(queue_key);
+    }
+  }
+
+  // Marks 'trajectory_id' as finished.
+  void FinishTrajectory(const int trajectory_id) {
+    for (const auto& queue_key : queue_keys_[trajectory_id]) {
+      queue_.MarkQueueAsFinished(queue_key);
+    }
+  }
+
+  // Adds 'sensor_data' for 'trajectory_id' to be collated. 'sensor_data' must
+  // contain valid sensor data. Sensor packets with matching 'sensor_id' must be
+  // added in time order.
+  void AddSensorData(const int trajectory_id, const int64 timestamp,
+                     const string& sensor_id,
+                     std::unique_ptr<SensorDataType> sensor_data) {
+    sensor_packet_period_histogram_builder_.Add(trajectory_id, timestamp,
+                                                sensor_id);
+    queue_.Add(
+        SensorCollatorQueueKey{trajectory_id, sensor_id}, timestamp,
+        common::make_unique<Value>(Value{timestamp, std::move(sensor_data)}));
+  }
+
+  // Dispatches all queued sensor packets. May only be called once.
+  // AddSensorData may not be called after Flush.
+  void Flush() {
+    queue_.Flush();
+    sensor_packet_period_histogram_builder_.LogHistogramsAndClear();
+  }
+
+  // Returns the number of packets associated with 'trajectory_id' that are
+  // available for processing.
+  int num_available_packets(const int trajectory_id) {
+    int num = std::numeric_limits<int>::max();
+    for (const auto& queue_key : queue_keys_[trajectory_id]) {
+      num = std::min(num, queue_.num_available(queue_key));
+    }
+    return num;
+  }
+
+ private:
+  struct Value {
+    int64 timestamp;
+    std::unique_ptr<SensorDataType> sensor_data;
+  };
+
+  // Queue keys are a pair of trajectory ID and sensor identifier.
+  common::OrderedMultiQueue<SensorCollatorQueueKey, int64, Value> queue_;
+
+  // Map of trajectory ID to all associated QueueKeys.
+  std::unordered_map<int, std::vector<SensorCollatorQueueKey>> queue_keys_;
+  sensor::SensorPacketPeriodHistogramBuilder
+      sensor_packet_period_histogram_builder_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_

cartographer/cartographer/mapping/sensor_collator_test.cc

@@ -0,0 +1,86 @@
+/*
+ * 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/sensor_collator.h"
+
+#include <memory>
+
+#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/proto/sensor.pb.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+struct TestData {
+  string frame_id;
+};
+
+TEST(SensorCollator, Ordering) {
+  TestData first{"horizontal_laser"};
+  TestData second{"vertical_laser"};
+  TestData third{"imu"};
+  TestData fourth{"horizontal_laser"};
+  TestData fifth{"vertical_laser"};
+  TestData sixth{"something"};
+
+  const std::unordered_set<string> frame_ids = {
+      "horizontal_laser", "vertical_laser", "imu", "something"};
+  std::vector<std::pair<int64, TestData>> received;
+  SensorCollator<TestData> sensor_collator;
+  sensor_collator.AddTrajectory(
+      0, frame_ids,
+      [&received](const int64 timestamp, std::unique_ptr<TestData> packet) {
+        received.push_back(std::make_pair(timestamp, *packet));
+      });
+
+  sensor_collator.AddSensorData(0, 100, first.frame_id,
+                                common::make_unique<TestData>(first));
+  sensor_collator.AddSensorData(0, 600, sixth.frame_id,
+                                common::make_unique<TestData>(sixth));
+  sensor_collator.AddSensorData(0, 400, fourth.frame_id,
+                                common::make_unique<TestData>(fourth));
+  sensor_collator.AddSensorData(0, 200, second.frame_id,
+                                common::make_unique<TestData>(second));
+  sensor_collator.AddSensorData(0, 500, fifth.frame_id,
+                                common::make_unique<TestData>(fifth));
+  sensor_collator.AddSensorData(0, 300, third.frame_id,
+                                common::make_unique<TestData>(third));
+
+  EXPECT_EQ(3, received.size());
+  EXPECT_EQ(100, received[0].first);
+  EXPECT_EQ("horizontal_laser", received[0].second.frame_id);
+  EXPECT_EQ(200, received[1].first);
+  EXPECT_EQ("vertical_laser", received[1].second.frame_id);
+  EXPECT_EQ(300, received[2].first);
+  EXPECT_EQ("imu", received[2].second.frame_id);
+
+  sensor_collator.Flush();
+
+  ASSERT_EQ(6, received.size());
+  EXPECT_EQ("horizontal_laser", received[3].second.frame_id);
+  EXPECT_EQ(500, received[4].first);
+  EXPECT_EQ("vertical_laser", received[4].second.frame_id);
+  EXPECT_EQ(600, received[5].first);
+  EXPECT_EQ("something", received[5].second.frame_id);
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/sparse_pose_graph.cc

@@ -0,0 +1,65 @@
+/*
+ * 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/sparse_pose_graph.h"
+
+#include <unordered_map>
+
+#include "cartographer/mapping/sparse_pose_graph/constraint_builder.h"
+#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
+    const std::vector<TrajectoryNode>& trajectory_nodes) {
+  std::vector<std::vector<TrajectoryNode>> trajectories;
+  std::unordered_map<const mapping::Submaps*, int> trajectory_ids;
+  for (const auto& node : trajectory_nodes) {
+    const auto* trajectory = node.constant_data->trajectory;
+    if (trajectory_ids.emplace(trajectory, trajectories.size()).second) {
+      trajectories.push_back({node});
+    } else {
+      trajectories[trajectory_ids[trajectory]].push_back(node);
+    }
+  }
+  return trajectories;
+}
+
+proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::SparsePoseGraphOptions options;
+  options.set_optimize_every_n_scans(
+      parameter_dictionary->GetInt("optimize_every_n_scans"));
+  options.set_also_match_to_new_submaps(
+      parameter_dictionary->GetBool("also_match_to_new_submaps"));
+  *options.mutable_constraint_builder_options() =
+      sparse_pose_graph::CreateConstraintBuilderOptions(
+          parameter_dictionary->GetDictionary("constraint_builder").get());
+  *options.mutable_optimization_problem_options() =
+      mapping::sparse_pose_graph::CreateOptimizationProblemOptions(
+          parameter_dictionary->GetDictionary("optimization_problem").get());
+  options.set_max_num_final_iterations(
+      parameter_dictionary->GetNonNegativeInt("max_num_final_iterations"));
+  CHECK_GT(options.max_num_final_iterations(), 0);
+  options.set_global_sampling_ratio(
+      parameter_dictionary->GetDouble("global_sampling_ratio"));
+  return options;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/sparse_pose_graph.h

@@ -0,0 +1,127 @@
+/*
+ * 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_SPARSE_POSE_GRAPH_H_
+#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
+
+#include <vector>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/mapping/proto/scan_matching_progress.pb.h"
+#include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
+#include "cartographer/mapping/submaps.h"
+#include "cartographer/mapping/trajectory_node.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
+    common::LuaParameterDictionary* const parameter_dictionary);
+
+// Splits TrajectoryNodes by ID.
+std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
+    const std::vector<TrajectoryNode>& trajectory_nodes);
+
+class SparsePoseGraph {
+ public:
+  // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
+  // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
+  // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
+  struct Constraint2D {
+    struct Pose {
+      transform::Rigid2d zbar_ij;
+      Eigen::Matrix<double, 3, 3> sqrt_Lambda_ij;
+    };
+
+    // Submap index.
+    int i;
+
+    // Scan index.
+    int j;
+
+    // Pose of the scan 'j' relative to submap 'i'.
+    Pose pose;
+
+    // Differentiates between intra-submap (where scan 'j' was inserted into
+    // submap 'i') and inter-submap constraints (where scan 'j' was not inserted
+    // into submap 'i').
+    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
+  };
+
+  struct Constraint3D {
+    struct Pose {
+      transform::Rigid3d zbar_ij;
+      Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
+    };
+
+    // Submap index.
+    int i;
+
+    // Scan index.
+    int j;
+
+    // Pose of the scan 'j' relative to submap 'i'.
+    Pose pose;
+
+    // Differentiates between intra-submap (where scan 'j' was inserted into
+    // submap 'i') and inter-submap constraints (where scan 'j' was not inserted
+    // into submap 'i').
+    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
+  };
+
+  SparsePoseGraph() {}
+  virtual ~SparsePoseGraph() {}
+
+  SparsePoseGraph(const SparsePoseGraph&) = delete;
+  SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
+
+  // Computes optimized poses.
+  virtual void RunFinalOptimization() = 0;
+
+  // Will once return true whenever new optimized poses are available.
+  virtual bool HasNewOptimizedPoses() = 0;
+
+  // Returns the scan matching progress.
+  virtual proto::ScanMatchingProgress GetScanMatchingProgress() = 0;
+
+  // Get the current trajectory clusters.
+  virtual std::vector<std::vector<const Submaps*>>
+  GetConnectedTrajectories() = 0;
+
+  // Returns the current optimized transforms for the given 'submaps'.
+  virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
+      const Submaps& submaps) = 0;
+
+  // Returns the transform converting from odometry based transforms to the
+  // optimized world.
+  virtual transform::Rigid3d GetOdometryToMapTransform(
+      const mapping::Submaps& submaps) = 0;
+
+  // Returns the current optimized trajectory.
+  virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
+
+  // Returns the collection of 2D constraints.
+  virtual std::vector<Constraint2D> constraints_2d() = 0;
+
+  // Returns the collection of 3D constraints.
+  virtual std::vector<Constraint3D> constraints_3d() = 0;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_

cartographer/cartographer/mapping/sparse_pose_graph/CMakeLists.txt

@@ -0,0 +1,41 @@
+# 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.
+
+add_subdirectory("proto")
+
+google_library(mapping_sparse_pose_graph_constraint_builder
+  SRCS
+    constraint_builder.cc
+  HDRS
+    constraint_builder.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    mapping_2d_scan_matching_ceres_scan_matcher
+    mapping_2d_scan_matching_fast_correlative_scan_matcher
+    mapping_3d_scan_matching_ceres_scan_matcher
+    mapping_3d_scan_matching_fast_correlative_scan_matcher
+    mapping_sparse_pose_graph_proto_constraint_builder_options
+    sensor_voxel_filter
+)
+
+google_library(mapping_sparse_pose_graph_optimization_problem_options
+  SRCS
+    optimization_problem_options.cc
+  HDRS
+    optimization_problem_options.h
+  DEPENDS
+    common_ceres_solver_options
+    common_lua_parameter_dictionary
+    mapping_sparse_pose_graph_proto_optimization_problem_options
+)

cartographer/cartographer/mapping/sparse_pose_graph/constraint_builder.cc

@@ -0,0 +1,66 @@
+/*
+ * 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/sparse_pose_graph/constraint_builder.h"
+
+#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
+#include "cartographer/sensor/voxel_filter.h"
+
+namespace cartographer {
+namespace mapping {
+namespace sparse_pose_graph {
+
+proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::ConstraintBuilderOptions options;
+  options.set_sampling_ratio(parameter_dictionary->GetDouble("sampling_ratio"));
+  options.set_max_constraint_distance(
+      parameter_dictionary->GetDouble("max_constraint_distance"));
+  *options.mutable_adaptive_voxel_filter_options() =
+      sensor::CreateAdaptiveVoxelFilterOptions(
+          parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
+  options.set_min_score(parameter_dictionary->GetDouble("min_score"));
+  options.set_global_localization_min_score(
+      parameter_dictionary->GetDouble("global_localization_min_score"));
+  options.set_max_covariance_trace(
+      parameter_dictionary->GetDouble("max_covariance_trace"));
+  options.set_lower_covariance_eigenvalue_bound(
+      parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound"));
+  options.set_log_matches(parameter_dictionary->GetBool("log_matches"));
+  *options.mutable_fast_correlative_scan_matcher_options() =
+      mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
+          parameter_dictionary->GetDictionary("fast_correlative_scan_matcher")
+              .get());
+  *options.mutable_ceres_scan_matcher_options() =
+      mapping_2d::scan_matching::CreateCeresScanMatcherOptions(
+          parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
+  *options.mutable_fast_correlative_scan_matcher_options_3d() =
+      mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions(
+          parameter_dictionary
+              ->GetDictionary("fast_correlative_scan_matcher_3d")
+              .get());
+  *options.mutable_ceres_scan_matcher_options_3d() =
+      mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
+          parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get());
+  return options;
+}
+
+}  // namespace sparse_pose_graph
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/sparse_pose_graph/constraint_builder.h

@@ -0,0 +1,34 @@
+/*
+ * 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_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_
+#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
+
+namespace cartographer {
+namespace mapping {
+namespace sparse_pose_graph {
+
+proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
+    common::LuaParameterDictionary* parameter_dictionary);
+
+}  // namespace sparse_pose_graph
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_

cartographer/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc

@@ -0,0 +1,50 @@
+/*
+ * 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/sparse_pose_graph/optimization_problem_options.h"
+
+#include "cartographer/common/ceres_solver_options.h"
+
+namespace cartographer {
+namespace mapping {
+namespace sparse_pose_graph {
+
+proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::OptimizationProblemOptions options;
+  options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale"));
+  options.set_acceleration_scale(
+      parameter_dictionary->GetDouble("acceleration_scale"));
+  options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale"));
+  options.set_consecutive_scan_translation_penalty_factor(
+      parameter_dictionary->GetDouble(
+          "consecutive_scan_translation_penalty_factor"));
+  options.set_consecutive_scan_rotation_penalty_factor(
+      parameter_dictionary->GetDouble(
+          "consecutive_scan_rotation_penalty_factor"));
+  options.set_log_solver_summary(
+      parameter_dictionary->GetBool("log_solver_summary"));
+  options.set_log_residual_histograms(
+      parameter_dictionary->GetBool("log_residual_histograms"));
+  *options.mutable_ceres_solver_options() =
+      common::CreateCeresSolverOptionsProto(
+          parameter_dictionary->GetDictionary("ceres_solver_options").get());
+  return options;
+}
+
+}  // namespace sparse_pose_graph
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/sparse_pose_graph/optimization_problem_options.h

@@ -0,0 +1,34 @@
+/*
+ * 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_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
+#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
+
+namespace cartographer {
+namespace mapping {
+namespace sparse_pose_graph {
+
+proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
+    common::LuaParameterDictionary* parameter_dictionary);
+
+}  // namespace sparse_pose_graph
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_

cartographer/cartographer/mapping/sparse_pose_graph/proto/CMakeLists.txt

@@ -0,0 +1,31 @@
+# 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.
+
+google_proto_library(mapping_sparse_pose_graph_proto_constraint_builder_options
+  SRCS
+    constraint_builder_options.proto
+  DEPENDS
+    mapping_2d_scan_matching_proto_ceres_scan_matcher_options
+    mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
+    mapping_3d_scan_matching_proto_ceres_scan_matcher_options
+    mapping_3d_scan_matching_proto_fast_correlative_scan_matcher_options
+    sensor_proto_adaptive_voxel_filter_options
+)
+
+google_proto_library(mapping_sparse_pose_graph_proto_optimization_problem_options
+  SRCS
+    optimization_problem_options.proto
+  DEPENDS
+    common_proto_ceres_solver_options
+)

cartographer/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto

@@ -0,0 +1,63 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping.sparse_pose_graph.proto;
+
+import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
+import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
+import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
+import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
+import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
+
+message ConstraintBuilderOptions {
+  // A constraint will be added if the proportion of added constraints to
+  // potential constraints drops below this number.
+  optional double sampling_ratio = 1;
+
+  // Threshold for poses to be considered near a submap.
+  optional double max_constraint_distance = 2;
+
+  // Voxel filter used to compute a sparser point cloud for matching.
+  optional sensor.proto.AdaptiveVoxelFilterOptions
+      adaptive_voxel_filter_options = 3;
+
+  // Threshold for the scan match score below which a match is not considered.
+  // Low scores indicate that the scan and map do not look similar.
+  optional double min_score = 4;
+
+  // Threshold below which global localizations are not trusted.
+  optional double global_localization_min_score = 5;
+
+  // Threshold for the covariance trace above which a match is not considered.
+  // High traces indicate that the match is not very unique.
+  optional double max_covariance_trace = 6;
+
+  // Lower bound for covariance eigenvalues to limit the weight of matches.
+  optional double lower_covariance_eigenvalue_bound = 7;
+
+  // If enabled, logs information of loop-closing constraints for debugging.
+  optional bool log_matches = 8;
+
+  // Options for the internally used scan matchers.
+  optional mapping_2d.scan_matching.proto.FastCorrelativeScanMatcherOptions
+      fast_correlative_scan_matcher_options = 9;
+  optional mapping_2d.scan_matching.proto.CeresScanMatcherOptions
+      ceres_scan_matcher_options = 11;
+  optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
+      fast_correlative_scan_matcher_options_3d = 10;
+  optional mapping_3d.scan_matching.proto.CeresScanMatcherOptions
+      ceres_scan_matcher_options_3d = 12;
+}

cartographer/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto

@@ -0,0 +1,44 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping.sparse_pose_graph.proto;
+
+import "cartographer/common/proto/ceres_solver_options.proto";
+
+// NEXT ID: 11
+message OptimizationProblemOptions {
+  // Scaling parameter for Huber loss function.
+  optional double huber_scale = 1;
+
+  // Scaling parameter for the IMU acceleration term.
+  optional double acceleration_scale = 8;
+
+  // Scaling parameter for the IMU rotation term.
+  optional double rotation_scale = 9;
+
+  // Penalty factors for changes to the relative pose between consecutive scans.
+  optional double consecutive_scan_translation_penalty_factor = 2;
+  optional double consecutive_scan_rotation_penalty_factor = 3;
+
+  // If true, the Ceres solver summary will be logged for every optimization.
+  optional bool log_solver_summary = 5;
+
+  // If true, histograms of the final residual values after optimization will be
+  // logged for every optimization.
+  optional bool log_residual_histograms = 10;
+
+  optional common.proto.CeresSolverOptions ceres_solver_options = 7;
+}

cartographer/cartographer/mapping/sparse_pose_graph_test.cc

@@ -0,0 +1,68 @@
+/*
+ * 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/sparse_pose_graph.h"
+
+#include <deque>
+
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+class FakeSubmaps : public Submaps {
+ public:
+  const Submap* Get(int) const override { LOG(FATAL) << "Not implemented."; }
+
+  int size() const override { LOG(FATAL) << "Not implemented."; }
+
+  void SubmapToProto(int, const std::vector<mapping::TrajectoryNode>&,
+                     const transform::Rigid3d&,
+                     proto::SubmapQuery::Response*) override {
+    LOG(FATAL) << "Not implemented.";
+  }
+};
+
+TEST(SparsePoseGraphTest, SplitTrajectoryNodes) {
+  std::vector<TrajectoryNode> trajectory_nodes;
+  const std::vector<FakeSubmaps> submaps(5);
+  std::deque<TrajectoryNode::ConstantData> constant_data;
+  constexpr int kNumTrajectoryNodes = 10;
+  for (int j = 0; j < kNumTrajectoryNodes; ++j) {
+    for (size_t i = 0; i < submaps.size(); ++i) {
+      constant_data.push_back({});
+      constant_data.back().trajectory = &submaps[i];
+      TrajectoryNode node;
+      node.constant_data = &constant_data.back();
+      trajectory_nodes.push_back(node);
+    }
+  }
+  std::vector<std::vector<TrajectoryNode>> split_trajectory_nodes =
+      SplitTrajectoryNodes(trajectory_nodes);
+  EXPECT_EQ(split_trajectory_nodes.size(), submaps.size());
+  for (size_t i = 0; i < submaps.size(); ++i) {
+    EXPECT_EQ(split_trajectory_nodes[i].size(), kNumTrajectoryNodes);
+    for (const auto& node : split_trajectory_nodes[i]) {
+      EXPECT_EQ(node.constant_data->trajectory, &submaps[i]);
+    }
+  }
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/submaps.cc

@@ -0,0 +1,93 @@
+/*
+ * 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/submaps.h"
+
+#include <vector>
+
+#include "cartographer/common/port.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+constexpr uint8 Submaps::kUnknownLogOdds;
+
+Submaps::Submaps() {}
+
+Submaps::~Submaps() {}
+
+int Submaps::matching_index() const {
+  if (size() > 1) {
+    return size() - 2;
+  }
+  return size() - 1;
+}
+
+std::vector<int> Submaps::insertion_indices() const {
+  if (size() > 1) {
+    return {size() - 2, size() - 1};
+  }
+  return {size() - 1};
+}
+
+void Submaps::AddProbabilityGridToResponse(
+    const transform::Rigid3d& local_submap_pose,
+    const mapping_2d::ProbabilityGrid& probability_grid,
+    proto::SubmapQuery::Response* response) {
+  Eigen::Array2i offset;
+  mapping_2d::CellLimits limits;
+  probability_grid.ComputeCroppedLimits(&offset, &limits);
+
+  string cells;
+  for (const Eigen::Array2i& xy_index :
+       mapping_2d::XYIndexRangeIterator(limits)) {
+    if (probability_grid.IsKnown(xy_index + offset)) {
+      // We would like to add 'delta' but this is not possible using a value and
+      // alpha. We use premultiplied alpha, so when 'delta' is positive we can
+      // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
+      // zero, and use 'alpha' to subtract. This is only correct when the pixel
+      // is currently white, so walls will look too gray. This should be hard to
+      // detect visually for the user, though.
+      const int delta =
+          128 - ProbabilityToLogOddsInteger(
+                    probability_grid.GetProbability(xy_index + offset));
+      const uint8 alpha = delta > 0 ? 0 : -delta;
+      const uint8 value = delta > 0 ? delta : 0;
+      cells.push_back(value);
+      cells.push_back((value || alpha) ? alpha : 1);
+    } else {
+      cells.push_back(static_cast<uint8>(kUnknownLogOdds));  // value
+      cells.push_back(0);                                    // alpha
+    }
+  }
+  common::FastGzipString(cells, response->mutable_cells());
+
+  response->set_width(limits.num_x_cells);
+  response->set_height(limits.num_y_cells);
+  const double resolution = probability_grid.limits().resolution();
+  response->set_resolution(resolution);
+  const double max_x = probability_grid.limits().edge_limits().max().x() -
+                       resolution * offset.y();
+  const double max_y = probability_grid.limits().edge_limits().max().y() -
+                       resolution * offset.x();
+  *response->mutable_slice_pose() = transform::ToProto(
+      local_submap_pose.inverse() *
+      transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/submaps.h

@@ -0,0 +1,136 @@
+/*
+ * 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.
+ */
+
+// Submaps is a sequence of maps to which scans are matched and into which scans
+// are inserted.
+//
+// Except during initialization when only a single submap exists, there are
+// always two submaps into which scans are inserted: an old submap that is used
+// for matching, and a new one, which will be used for matching next, that is
+// being initialized.
+//
+// Once a certain number of scans have been inserted, the new submap is
+// considered initialized: the old submap is no longer changed, the "new" submap
+// is now the "old" submap and is used for scan-to-map matching. Moreover,
+// a "new" submap gets inserted.
+#ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_
+#define CARTOGRAPHER_MAPPING_SUBMAPS_H_
+
+#include <memory>
+#include <vector>
+
+#include "Eigen/Geometry"
+#include "cartographer/common/math.h"
+#include "cartographer/common/port.h"
+#include "cartographer/mapping/probability_values.h"
+#include "cartographer/mapping/proto/submaps.pb.h"
+#include "cartographer/mapping/trajectory_node.h"
+#include "cartographer/mapping_2d/probability_grid.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+// Converts the given probability to log odds.
+inline float Logit(float probability) {
+  return std::log(probability / (1.f - probability));
+}
+
+const float kMaxLogOdds = Logit(kMaxProbability);
+const float kMinLogOdds = Logit(kMinProbability);
+
+// Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
+// kMaxLogOdds] is mapped to [1, 255].
+inline uint8 ProbabilityToLogOddsInteger(const float probability) {
+  const int value = common::RoundToInt((Logit(probability) - kMinLogOdds) *
+                                       254.f / (kMaxLogOdds - kMinLogOdds)) +
+                    1;
+  CHECK_LE(1, value);
+  CHECK_GE(255, value);
+  return value;
+}
+
+// An individual submap, which has an initial position 'origin', keeps track of
+// which laser fans where inserted into it, and sets the
+// 'finished_probability_grid' to be used for loop closing once the map no
+// longer changes.
+struct Submap {
+  Submap(const Eigen::Vector3f& origin, int begin_laser_fan_index)
+      : origin(origin),
+        begin_laser_fan_index(begin_laser_fan_index),
+        end_laser_fan_index(begin_laser_fan_index) {}
+
+  transform::Rigid3d local_pose() const {
+    return transform::Rigid3d::Translation(origin.cast<double>());
+  }
+
+  // Origin of this submap.
+  Eigen::Vector3f origin;
+
+  // This Submap contains LaserFans with indices in the range
+  // ['begin_laser_fan_index', 'end_laser_fan_index').
+  int begin_laser_fan_index;
+  int end_laser_fan_index;
+
+  // The 'finished_probability_grid' when this submap is finished and will not
+  // change anymore. Otherwise, this is nullptr and the next call to
+  // InsertLaserFan() will change the submap.
+  const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
+};
+
+// A container of Submaps.
+class Submaps {
+ public:
+  static constexpr uint8 kUnknownLogOdds = 0;
+
+  Submaps();
+  virtual ~Submaps();
+
+  Submaps(const Submaps&) = delete;
+  Submaps& operator=(const Submaps&) = delete;
+
+  // Returns the index of the newest initialized Submap which can be
+  // used for scan-to-map matching.
+  int matching_index() const;
+
+  // Returns the indices of the Submap into which point clouds will
+  // be inserted.
+  std::vector<int> insertion_indices() const;
+
+  // Returns the Submap with the given 'index'. The same 'index' will always
+  // return the same pointer, so that Submaps can be identified by it.
+  virtual const Submap* Get(int index) const = 0;
+
+  // Returns the number of Submaps.
+  virtual int size() const = 0;
+
+  // Fills data about the Submap with 'index' into the 'response'.
+  virtual void SubmapToProto(
+      int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
+      const transform::Rigid3d& global_submap_pose,
+      proto::SubmapQuery::Response* response) = 0;
+
+ protected:
+  static void AddProbabilityGridToResponse(
+      const transform::Rigid3d& local_submap_pose,
+      const mapping_2d::ProbabilityGrid& probability_grid,
+      proto::SubmapQuery::Response* response);
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_SUBMAPS_H_

cartographer/cartographer/mapping/submaps_test.cc

@@ -0,0 +1,42 @@
+/*
+ * 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/submaps.h"
+
+#include <cmath>
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+// Converts the given log odds to a probability. This function is known to be
+// very slow, because expf is incredibly slow.
+inline float Expit(float log_odds) {
+  const float exp_log_odds = std::exp(log_odds);
+  return exp_log_odds / (1.f + exp_log_odds);
+}
+
+TEST(SubmapsTest, LogOddsConversions) {
+  EXPECT_NEAR(Expit(Logit(kMinProbability)), kMinProbability, 1e-6);
+  EXPECT_NEAR(Expit(Logit(kMaxProbability)), kMaxProbability, 1e-6);
+  EXPECT_NEAR(Expit(Logit(0.5)), 0.5, 1e-6);
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/trajectory_connectivity.cc

@@ -0,0 +1,149 @@
+/*
+ * 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/trajectory_connectivity.h"
+
+#include <algorithm>
+#include <unordered_set>
+
+#include "cartographer/mapping/proto/trajectory_connectivity.pb.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+TrajectoryConnectivity::TrajectoryConnectivity()
+    : lock_(), forest_(), connection_map_() {}
+
+void TrajectoryConnectivity::Add(const Submaps* trajectory) {
+  CHECK(trajectory != nullptr);
+  common::MutexLocker locker(&lock_);
+  forest_.emplace(trajectory, trajectory);
+}
+
+void TrajectoryConnectivity::Connect(const Submaps* trajectory_a,
+                                     const Submaps* trajectory_b) {
+  CHECK(trajectory_a != nullptr);
+  CHECK(trajectory_b != nullptr);
+  common::MutexLocker locker(&lock_);
+  Union(trajectory_a, trajectory_b);
+  auto sorted_pair =
+      std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>());
+  ++connection_map_[sorted_pair];
+}
+
+void TrajectoryConnectivity::Union(const Submaps* const trajectory_a,
+                                   const Submaps* const trajectory_b) {
+  forest_.emplace(trajectory_a, trajectory_a);
+  forest_.emplace(trajectory_b, trajectory_b);
+  const Submaps* const representative_a = FindSet(trajectory_a);
+  const Submaps* const representative_b = FindSet(trajectory_b);
+  forest_[representative_a] = representative_b;
+}
+
+const Submaps* TrajectoryConnectivity::FindSet(
+    const Submaps* const trajectory) {
+  auto it = forest_.find(trajectory);
+  CHECK(it != forest_.end());
+  if (it->first != it->second) {
+    it->second = FindSet(it->second);
+  }
+  return it->second;
+}
+
+bool TrajectoryConnectivity::TransitivelyConnected(
+    const Submaps* trajectory_a, const Submaps* trajectory_b) {
+  CHECK(trajectory_a != nullptr);
+  CHECK(trajectory_b != nullptr);
+  common::MutexLocker locker(&lock_);
+
+  if (forest_.count(trajectory_a) == 0 || forest_.count(trajectory_b) == 0) {
+    return false;
+  }
+  return FindSet(trajectory_a) == FindSet(trajectory_b);
+}
+
+std::vector<std::vector<const Submaps*>>
+TrajectoryConnectivity::ConnectedComponents() {
+  // Map from cluster exemplar -> growing cluster.
+  std::unordered_map<const Submaps*, std::vector<const Submaps*>> map;
+  common::MutexLocker locker(&lock_);
+  for (const auto& trajectory_entry : forest_) {
+    map[FindSet(trajectory_entry.first)].push_back(trajectory_entry.first);
+  }
+
+  std::vector<std::vector<const Submaps*>> result;
+  result.reserve(map.size());
+  for (auto& pair : map) {
+    result.emplace_back(std::move(pair.second));
+  }
+  return result;
+}
+
+int TrajectoryConnectivity::ConnectionCount(const Submaps* trajectory_a,
+                                            const Submaps* trajectory_b) {
+  CHECK(trajectory_a != nullptr);
+  CHECK(trajectory_b != nullptr);
+  common::MutexLocker locker(&lock_);
+  const auto it = connection_map_.find(
+      std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>()));
+  return it != connection_map_.end() ? it->second : 0;
+}
+
+proto::TrajectoryConnectivity ToProto(
+    std::vector<std::vector<const Submaps*>> connected_components,
+    std::unordered_map<const mapping::Submaps*, int> trajectory_indices) {
+  proto::TrajectoryConnectivity proto;
+  std::vector<std::vector<int>> connected_components_by_indices;
+  for (const auto& connected_component : connected_components) {
+    connected_components_by_indices.emplace_back();
+    for (const mapping::Submaps* trajectory : connected_component) {
+      connected_components_by_indices.back().push_back(
+          trajectory_indices.at(trajectory));
+    }
+    std::sort(connected_components_by_indices.back().begin(),
+              connected_components_by_indices.back().end());
+  }
+  std::sort(connected_components_by_indices.begin(),
+            connected_components_by_indices.end());
+  for (const auto& connected_component : connected_components_by_indices) {
+    auto* proto_connected_component = proto.add_connected_component();
+    for (const int trajectory_id : connected_component) {
+      proto_connected_component->add_trajectory_id(trajectory_id);
+    }
+  }
+  return proto;
+}
+
+proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
+    const proto::TrajectoryConnectivity& trajectory_connectivity,
+    const int trajectory_id) {
+  for (const auto& connected_component :
+       trajectory_connectivity.connected_component()) {
+    if (std::find(connected_component.trajectory_id().begin(),
+                  connected_component.trajectory_id().end(),
+                  trajectory_id) != connected_component.trajectory_id().end()) {
+      return connected_component;
+    }
+  }
+
+  proto::TrajectoryConnectivity::ConnectedComponent connected_component;
+  connected_component.add_trajectory_id(trajectory_id);
+  return connected_component;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/trajectory_connectivity.h

@@ -0,0 +1,98 @@
+/*
+ * 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_CONNECTIVITY_H_
+#define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_
+
+#include <map>
+#include <unordered_map>
+
+#include "cartographer/common/mutex.h"
+#include "cartographer/mapping/proto/trajectory_connectivity.pb.h"
+#include "cartographer/mapping/submaps.h"
+
+namespace cartographer {
+namespace mapping {
+
+// A class that tracks the connectivity structure between trajectories.
+//
+// Connectivity includes both the count ("How many times have I _directly_
+// connected trajectories i and j?") and the transitive connectivity.
+//
+// This class is thread-safe.
+class TrajectoryConnectivity {
+ public:
+  TrajectoryConnectivity();
+
+  TrajectoryConnectivity(const TrajectoryConnectivity&) = delete;
+  TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete;
+
+  // Add a trajectory which is initially connected to nothing.
+  void Add(const Submaps* trajectory) EXCLUDES(lock_);
+
+  // Connect two trajectories. If either trajectory is untracked, it will be
+  // tracked. This function is invariant to the order of its arguments. Repeated
+  // calls to Connect increment the connectivity count.
+  void Connect(const Submaps* trajectory_a, const Submaps* trajectory_b)
+      EXCLUDES(lock_);
+
+  // Determines if two trajectories have been (transitively) connected. If
+  // either trajectory is not being tracked, returns false. This function is
+  // invariant to the order of its arguments.
+  bool TransitivelyConnected(const Submaps* trajectory_a,
+                             const Submaps* trajectory_b) EXCLUDES(lock_);
+
+  // Return the number of _direct_ connections between trajectory_a and
+  // trajectory_b. If either trajectory is not being tracked, returns 0. This
+  // function is invariant to the order of its arguments.
+  int ConnectionCount(const Submaps* trajectory_a, const Submaps* trajectory_b)
+      EXCLUDES(lock_);
+
+  // The trajectories, grouped by connectivity.
+  std::vector<std::vector<const Submaps*>> ConnectedComponents()
+      EXCLUDES(lock_);
+
+ private:
+  // Find the representative and compresses the path to it.
+  const Submaps* FindSet(const Submaps* trajectory) REQUIRES(lock_);
+  void Union(const Submaps* trajectory_a, const Submaps* trajectory_b)
+      REQUIRES(lock_);
+
+  common::Mutex lock_;
+  // Tracks transitive connectivity using a disjoint set forest, i.e. each
+  // entry points towards the representative for the given trajectory.
+  std::map<const Submaps*, const Submaps*> forest_ GUARDED_BY(lock_);
+  // Tracks the number of direct connections between a pair of trajectories.
+  std::map<std::pair<const Submaps*, const Submaps*>, int> connection_map_
+      GUARDED_BY(lock_);
+};
+
+// Returns a proto encoding connected components according to
+// 'trajectory_indices'.
+proto::TrajectoryConnectivity ToProto(
+    std::vector<std::vector<const Submaps*>> connected_components,
+    std::unordered_map<const mapping::Submaps*, int> trajectory_indices);
+
+// Returns the connected component containing 'trajectory_index'.
+proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
+    const cartographer::mapping::proto::TrajectoryConnectivity&
+        trajectory_connectivity,
+    int trajectory_id);
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_

cartographer/cartographer/mapping/trajectory_connectivity_test.cc

@@ -0,0 +1,135 @@
+/*
+ * 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/trajectory_connectivity.h"
+
+#include <algorithm>
+#include <memory>
+#include <vector>
+
+#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "cartographer/mapping_2d/submaps.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+class TrajectoryConnectivityTest : public ::testing::Test {
+ protected:
+  TrajectoryConnectivityTest() {
+    for (int i = 0; i < 10; ++i) {
+      auto parameter_dictionary = common::MakeDictionary(R"text(
+          return {
+            resolution = 0.05,
+            half_length = 10.,
+            num_laser_fans = 10,
+            output_debug_images = false,
+            laser_fan_inserter = {
+              insert_free_space = true,
+              hit_probability = 0.53,
+              miss_probability = 0.495,
+            },
+          })text");
+      trajectories_.emplace_back(new mapping_2d::Submaps(
+          mapping_2d::CreateSubmapsOptions(parameter_dictionary.get())));
+    }
+  }
+
+  // Helper function to avoid .get() noise.
+  const Submaps* trajectory(int index) { return trajectories_.at(index).get(); }
+
+  TrajectoryConnectivity trajectory_connectivity_;
+  std::vector<std::unique_ptr<const Submaps>> trajectories_;
+};
+
+TEST_F(TrajectoryConnectivityTest, TransitivelyConnected) {
+  // Make sure nothing's connected until we connect some things.
+  for (auto& trajectory_a : trajectories_) {
+    for (auto& trajectory_b : trajectories_) {
+      EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(
+          trajectory_a.get(), trajectory_b.get()));
+    }
+  }
+
+  // Connect some stuff up.
+  trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
+  EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
+                                                             trajectory(1)));
+  trajectory_connectivity_.Connect(trajectory(8), trajectory(9));
+  EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(8),
+                                                             trajectory(9)));
+  EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
+                                                              trajectory(9)));
+
+  trajectory_connectivity_.Connect(trajectory(1), trajectory(8));
+  for (int i : {0, 1}) {
+    for (int j : {8, 9}) {
+      EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(
+          trajectory(i), trajectory(j)));
+    }
+  }
+}
+
+TEST_F(TrajectoryConnectivityTest, EmptyConnectedComponents) {
+  auto connections = trajectory_connectivity_.ConnectedComponents();
+  EXPECT_EQ(0, connections.size());
+}
+
+TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
+  for (int i = 0; i <= 4; ++i) {
+    trajectory_connectivity_.Connect(trajectory(0), trajectory(i));
+  }
+  for (int i = 5; i <= 9; ++i) {
+    trajectory_connectivity_.Connect(trajectory(5), trajectory(i));
+  }
+  auto connections = trajectory_connectivity_.ConnectedComponents();
+  ASSERT_EQ(2, connections.size());
+  // The clustering is arbitrary; we need to figure out which one is which.
+  const std::vector<const Submaps*>* zero_cluster = nullptr;
+  const std::vector<const Submaps*>* five_cluster = nullptr;
+  if (std::find(connections[0].begin(), connections[0].end(), trajectory(0)) !=
+      connections[0].end()) {
+    zero_cluster = &connections[0];
+    five_cluster = &connections[1];
+  } else {
+    zero_cluster = &connections[1];
+    five_cluster = &connections[0];
+  }
+  for (int i = 0; i <= 9; ++i) {
+    EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(),
+                                trajectory(i)) != zero_cluster->end());
+    EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(),
+                               trajectory(i)) != five_cluster->end());
+  }
+}
+
+TEST_F(TrajectoryConnectivityTest, ConnectionCount) {
+  for (int i = 0; i < 10; ++i) {
+    trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
+    // Permute the arguments to check invariance.
+    EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1),
+                                                              trajectory(0)));
+  }
+  for (int i = 1; i < 9; ++i) {
+    EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
+                                                          trajectory(i + 1)));
+  }
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/cartographer/mapping/trajectory_node.h

@@ -0,0 +1,72 @@
+/*
+ * 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_NODE_H_
+#define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
+
+#include <deque>
+#include <vector>
+
+#include "Eigen/Core"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/laser.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+struct Submaps;
+
+struct TrajectoryNode {
+  struct ConstantData {
+    common::Time time;
+
+    // LaserFan in 'pose' frame. Only used in the 2D case.
+    sensor::LaserFan laser_fan;
+
+    // LaserFan in 'pose' frame. Only used in the 3D case.
+    sensor::CompressedLaserFan3D laser_fan_3d;
+
+    // Trajectory this node belongs to.
+    // TODO(jmason): The naming here is confusing because 'trajectory' doesn't
+    // seem like a good name for a Submaps*. Sort this out.
+    const Submaps* trajectory;
+
+    // Transform from the 3D 'tracking' frame to the 'pose' frame of the
+    // laser, which contains roll, pitch and height for 2D. In 3D this is
+    // always identity.
+    transform::Rigid3d tracking_to_pose;
+  };
+
+  common::Time time() const { return constant_data->time; }
+
+  const ConstantData* constant_data;
+
+  transform::Rigid3d pose;
+};
+
+// Users will only be interested in 'trajectory_nodes'. But 'constant_data'
+// is referenced by 'trajectory_nodes'. This struct guarantees that their
+// lifetimes are bound.
+struct TrajectoryNodes {
+  std::deque<mapping::TrajectoryNode::ConstantData> constant_data;
+  std::vector<mapping::TrajectoryNode> trajectory_nodes;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_

cartographer/cartographer/mapping_2d/CMakeLists.txt

@@ -0,0 +1,225 @@
+# 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.
+
+add_subdirectory("proto")
+add_subdirectory("scan_matching")
+add_subdirectory("sparse_pose_graph")
+
+google_library(mapping_2d_global_trajectory_builder
+  SRCS
+    global_trajectory_builder.cc
+  HDRS
+    global_trajectory_builder.h
+  DEPENDS
+    mapping_2d_local_trajectory_builder
+    mapping_2d_sparse_pose_graph
+    mapping_global_trajectory_builder_interface
+)
+
+google_library(mapping_2d_laser_fan_inserter
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    laser_fan_inserter.cc
+  HDRS
+    laser_fan_inserter.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_port
+    mapping_2d_probability_grid
+    mapping_2d_proto_laser_fan_inserter_options
+    mapping_2d_ray_casting
+    mapping_2d_xy_index
+    sensor_laser
+    sensor_point_cloud
+)
+
+google_library(mapping_2d_local_trajectory_builder
+  SRCS
+    local_trajectory_builder.cc
+  HDRS
+    local_trajectory_builder.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_make_unique
+    common_time
+    kalman_filter_pose_tracker
+    mapping_2d_proto_local_trajectory_builder_options
+    mapping_2d_scan_matching_ceres_scan_matcher
+    mapping_2d_scan_matching_real_time_correlative_scan_matcher
+    mapping_2d_submaps
+    mapping_3d_motion_filter
+    mapping_global_trajectory_builder_interface
+    sensor_configuration
+    sensor_laser
+    sensor_voxel_filter
+    transform_rigid_transform
+)
+
+google_library(mapping_2d_map_limits
+  USES_CERES
+  USES_EIGEN
+  HDRS
+    map_limits.h
+  DEPENDS
+    common_math
+    mapping_2d_xy_index
+    mapping_trajectory_node
+    sensor_laser
+    sensor_point_cloud
+    transform_rigid_transform
+    transform_transform
+)
+
+google_library(mapping_2d_probability_grid
+  USES_CERES
+  HDRS
+    probability_grid.h
+  DEPENDS
+    common_math
+    common_port
+    mapping_2d_map_limits
+    mapping_2d_xy_index
+    mapping_probability_values
+)
+
+google_library(mapping_2d_ray_casting
+  SRCS
+    ray_casting.cc
+  HDRS
+    ray_casting.h
+  DEPENDS
+    mapping_2d_map_limits
+    mapping_2d_xy_index
+    sensor_laser
+    sensor_point_cloud
+    transform_transform
+)
+
+google_library(mapping_2d_sparse_pose_graph
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    sparse_pose_graph.cc
+  HDRS
+    sparse_pose_graph.h
+  DEPENDS
+    common_fixed_ratio_sampler
+    common_make_unique
+    common_math
+    common_thread_pool
+    common_time
+    kalman_filter_pose_tracker
+    mapping_2d_sparse_pose_graph_constraint_builder
+    mapping_2d_sparse_pose_graph_optimization_problem
+    mapping_2d_submaps
+    mapping_proto_scan_matching_progress
+    mapping_sparse_pose_graph
+    mapping_trajectory_connectivity
+    sensor_compressed_point_cloud
+    sensor_point_cloud
+    sensor_voxel_filter
+    transform_transform
+)
+
+google_library(mapping_2d_submaps
+  USES_CERES
+  USES_EIGEN
+  USES_WEBP
+  SRCS
+    submaps.cc
+  HDRS
+    submaps.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_make_unique
+    mapping_2d_laser_fan_inserter
+    mapping_2d_map_limits
+    mapping_2d_probability_grid
+    mapping_2d_proto_submaps_options
+    mapping_proto_submaps
+    mapping_submaps
+    mapping_trajectory_node
+    sensor_laser
+    transform_rigid_transform
+)
+
+google_library(mapping_2d_xy_index
+  USES_CERES
+  USES_EIGEN
+  HDRS
+    xy_index.h
+  DEPENDS
+    common_math
+    common_port
+)
+
+google_test(mapping_2d_laser_fan_inserter_test
+  SRCS
+    laser_fan_inserter_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_lua_parameter_dictionary_test_helpers
+    common_make_unique
+    mapping_2d_laser_fan_inserter
+    mapping_2d_probability_grid
+)
+
+google_test(mapping_2d_map_limits_test
+  SRCS
+    map_limits_test.cc
+  DEPENDS
+    mapping_2d_map_limits
+)
+
+google_test(mapping_2d_probability_grid_test
+  SRCS
+    probability_grid_test.cc
+  DEPENDS
+    mapping_2d_probability_grid
+)
+
+google_test(mapping_2d_sparse_pose_graph_test
+  SRCS
+    sparse_pose_graph_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary_test_helpers
+    common_make_unique
+    common_thread_pool
+    common_time
+    mapping_2d_laser_fan_inserter
+    mapping_2d_sparse_pose_graph
+    mapping_2d_submaps
+    transform_rigid_transform
+    transform_rigid_transform_test_helpers
+    transform_transform
+)
+
+google_test(mapping_2d_submaps_test
+  SRCS
+    submaps_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_lua_parameter_dictionary_test_helpers
+    common_port
+    mapping_2d_submaps
+    transform_transform
+)
+
+google_test(mapping_2d_xy_index_test
+  SRCS
+    xy_index_test.cc
+  DEPENDS
+    mapping_2d_xy_index
+)

cartographer/cartographer/mapping_2d/global_trajectory_builder.cc

@@ -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.
+ */
+
+#include "cartographer/mapping_2d/global_trajectory_builder.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
+    const proto::LocalTrajectoryBuilderOptions& options,
+    SparsePoseGraph* sparse_pose_graph)
+    : options_(options),
+      sparse_pose_graph_(sparse_pose_graph),
+      local_trajectory_builder_(options) {}
+
+GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
+
+const Submaps* GlobalTrajectoryBuilder::submaps() const {
+  return local_trajectory_builder_.submaps();
+}
+
+Submaps* GlobalTrajectoryBuilder::submaps() {
+  return local_trajectory_builder_.submaps();
+}
+
+kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
+  return local_trajectory_builder_.pose_tracker();
+}
+
+void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
+    const common::Time time, const sensor::LaserFan3D& laser_fan) {
+  std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
+      local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
+  if (insertion_result != nullptr) {
+    sparse_pose_graph_->AddScan(
+        insertion_result->time, insertion_result->tracking_to_tracking_2d,
+        insertion_result->laser_fan_in_tracking_2d,
+        insertion_result->pose_estimate_2d,
+        kalman_filter::Project2D(insertion_result->covariance_estimate),
+        insertion_result->submaps, insertion_result->matching_submap,
+        insertion_result->insertion_submaps);
+  }
+}
+
+void GlobalTrajectoryBuilder::AddImuData(
+    const common::Time time, const Eigen::Vector3d& linear_acceleration,
+    const Eigen::Vector3d& angular_velocity) {
+  local_trajectory_builder_.AddImuData(time, linear_acceleration,
+                                       angular_velocity);
+}
+
+void GlobalTrajectoryBuilder::AddOdometerPose(
+    const common::Time time, const transform::Rigid3d& pose,
+    const kalman_filter::PoseCovariance& covariance) {
+  local_trajectory_builder_.AddOdometerPose(time, pose, covariance);
+}
+
+const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&
+GlobalTrajectoryBuilder::pose_estimate() const {
+  return local_trajectory_builder_.pose_estimate();
+}
+
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/cartographer/mapping_2d/global_trajectory_builder.h

@@ -0,0 +1,63 @@
+/*
+ * 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_2D_GLOBAL_TRAJECTORY_BUILDER_H_
+#define CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_
+
+#include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping_2d/local_trajectory_builder.h"
+#include "cartographer/mapping_2d/sparse_pose_graph.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+class GlobalTrajectoryBuilder
+    : public mapping::GlobalTrajectoryBuilderInterface {
+ public:
+  GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
+                          SparsePoseGraph* sparse_pose_graph);
+  ~GlobalTrajectoryBuilder() override;
+
+  GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
+  GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
+
+  const Submaps* submaps() const override;
+  Submaps* submaps() override;
+  kalman_filter::PoseTracker* pose_tracker() const override;
+  const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
+      const override;
+
+  void AddHorizontalLaserFan(common::Time time,
+                             const sensor::LaserFan3D& laser_fan) override;
+  void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
+                  const Eigen::Vector3d& angular_velocity) override;
+  void AddOdometerPose(
+      common::Time time, const transform::Rigid3d& pose,
+      const kalman_filter::PoseCovariance& covariance) override;
+  void AddLaserFan3D(common::Time, const sensor::LaserFan3D&) override {
+    LOG(FATAL) << "Not implemented.";
+  };
+
+ private:
+  const proto::LocalTrajectoryBuilderOptions options_;
+  SparsePoseGraph* const sparse_pose_graph_;
+  LocalTrajectoryBuilder local_trajectory_builder_;
+};
+
+}  // namespace mapping_2d
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_

cartographer/cartographer/mapping_2d/laser_fan_inserter.cc

@@ -0,0 +1,72 @@
+/*
+ * 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_2d/laser_fan_inserter.h"
+
+#include <cstdlib>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/mapping_2d/ray_casting.h"
+#include "cartographer/mapping_2d/xy_index.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+proto::LaserFanInserterOptions CreateLaserFanInserterOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::LaserFanInserterOptions options;
+  options.set_hit_probability(
+      parameter_dictionary->GetDouble("hit_probability"));
+  options.set_miss_probability(
+      parameter_dictionary->GetDouble("miss_probability"));
+  options.set_insert_free_space(
+      parameter_dictionary->HasKey("insert_free_space")
+          ? parameter_dictionary->GetBool("insert_free_space")
+          : true);
+  CHECK_GT(options.hit_probability(), 0.5);
+  CHECK_LT(options.miss_probability(), 0.5);
+  return options;
+}
+
+LaserFanInserter::LaserFanInserter(
+    const proto::LaserFanInserterOptions& options)
+    : options_(options),
+      hit_table_(mapping::ComputeLookupTableToApplyOdds(
+          mapping::Odds(options.hit_probability()))),
+      miss_table_(mapping::ComputeLookupTableToApplyOdds(
+          mapping::Odds(options.miss_probability()))) {}
+
+void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,
+                              ProbabilityGrid* const probability_grid) const {
+  CHECK_NOTNULL(probability_grid)->StartUpdate();
+
+  // By not starting a new update after hits are inserted, we give hits priority
+  // (i.e. no hits will be ignored because of a miss in the same cell).
+  CastRays(laser_fan, probability_grid->limits(),
+           [this, &probability_grid](const Eigen::Array2i& hit) {
+             probability_grid->ApplyLookupTable(hit, hit_table_);
+           },
+           [this, &probability_grid](const Eigen::Array2i& miss) {
+             if (options_.insert_free_space()) {
+               probability_grid->ApplyLookupTable(miss, miss_table_);
+             }
+           });
+}
+
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/cartographer/mapping_2d/laser_fan_inserter.h

@@ -0,0 +1,60 @@
+/*
+ * 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_2D_LASER_FAN_INSERTER_H_
+#define CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_
+
+#include <utility>
+#include <vector>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+#include "cartographer/mapping_2d/probability_grid.h"
+#include "cartographer/mapping_2d/proto/laser_fan_inserter_options.pb.h"
+#include "cartographer/mapping_2d/xy_index.h"
+#include "cartographer/sensor/laser.h"
+#include "cartographer/sensor/point_cloud.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+proto::LaserFanInserterOptions CreateLaserFanInserterOptions(
+    common::LuaParameterDictionary* parameter_dictionary);
+
+class LaserFanInserter {
+ public:
+  explicit LaserFanInserter(const proto::LaserFanInserterOptions& options);
+
+  LaserFanInserter(const LaserFanInserter&) = delete;
+  LaserFanInserter& operator=(const LaserFanInserter&) = delete;
+
+  // Inserts 'laser_fan' into 'probability_grid'.
+  void Insert(const sensor::LaserFan& laser_fan,
+              ProbabilityGrid* probability_grid) const;
+
+  const std::vector<uint16>& hit_table() const { return hit_table_; }
+  const std::vector<uint16>& miss_table() const { return miss_table_; }
+
+ private:
+  const proto::LaserFanInserterOptions options_;
+  const std::vector<uint16> hit_table_;
+  const std::vector<uint16> miss_table_;
+};
+
+}  // namespace mapping_2d
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_

cartographer/cartographer/mapping_2d/laser_fan_inserter_test.cc

@@ -0,0 +1,130 @@
+/*
+ * 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_2d/laser_fan_inserter.h"
+
+#include <memory>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/mapping_2d/probability_grid.h"
+#include "gmock/gmock.h"
+
+namespace cartographer {
+namespace mapping_2d {
+namespace {
+
+class LaserFanInserterTest : public ::testing::Test {
+ protected:
+  LaserFanInserterTest()
+      : probability_grid_(
+            MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.),
+                                              Eigen::Vector2d(0., 4.)))) {
+    const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
+    CHECK_EQ(5, cell_limits.num_x_cells);
+    CHECK_EQ(5, cell_limits.num_y_cells);
+
+    auto parameter_dictionary = common::MakeDictionary(
+        "return { "
+        "insert_free_space = true, "
+        "hit_probability = 0.7, "
+        "miss_probability = 0.4, "
+        "}");
+    options_ = CreateLaserFanInserterOptions(parameter_dictionary.get());
+    laser_fan_inserter_ = common::make_unique<LaserFanInserter>(options_);
+  }
+
+  void InsertPointCloud() {
+    sensor::LaserFan laser_fan;
+    laser_fan.point_cloud.emplace_back(-3.5, 0.5);
+    laser_fan.point_cloud.emplace_back(-2.5, 1.5);
+    laser_fan.point_cloud.emplace_back(-1.5, 2.5);
+    laser_fan.point_cloud.emplace_back(-0.5, 3.5);
+    laser_fan.origin.x() = -0.5;
+    laser_fan.origin.y() = 0.5;
+    probability_grid_.StartUpdate();
+    laser_fan_inserter_->Insert(laser_fan, &probability_grid_);
+  }
+
+  ProbabilityGrid probability_grid_;
+  std::unique_ptr<LaserFanInserter> laser_fan_inserter_;
+  proto::LaserFanInserterOptions options_;
+};
+
+TEST_F(LaserFanInserterTest, InsertPointCloud) {
+  InsertPointCloud();
+
+  const Eigen::AlignedBox2d& edge_limits =
+      probability_grid_.limits().edge_limits();
+  EXPECT_NEAR(-4., edge_limits.min().x(), 1e-9);
+  EXPECT_NEAR(0., edge_limits.min().y(), 1e-9);
+  EXPECT_NEAR(1., edge_limits.max().x(), 1e-9);
+  EXPECT_NEAR(5., edge_limits.max().y(), 1e-9);
+
+  const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
+  EXPECT_EQ(5, cell_limits.num_x_cells);
+  EXPECT_EQ(5, cell_limits.num_y_cells);
+
+  enum class State { UNKNOWN, MISS, HIT };
+  State expected_states[5][5] = {
+      {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN,
+       State::UNKNOWN},
+      {State::UNKNOWN, State::HIT, State::MISS, State::MISS, State::MISS},
+      {State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS, State::MISS},
+      {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS},
+      {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN,
+       State::HIT}};
+  for (int row = 0; row != 5; ++row) {
+    for (int column = 0; column != 5; ++column) {
+      Eigen::Array2i xy_index(row, column);
+      EXPECT_TRUE(probability_grid_.limits().Contains(xy_index));
+      switch (expected_states[column][row]) {
+        case State::UNKNOWN:
+          EXPECT_FALSE(probability_grid_.IsKnown(xy_index));
+          break;
+        case State::MISS:
+          EXPECT_NEAR(options_.miss_probability(),
+                      probability_grid_.GetProbability(xy_index), 1e-4);
+          break;
+        case State::HIT:
+          EXPECT_NEAR(options_.hit_probability(),
+                      probability_grid_.GetProbability(xy_index), 1e-4);
+          break;
+      }
+    }
+  }
+}
+
+TEST_F(LaserFanInserterTest, ProbabilityProgression) {
+  InsertPointCloud();
+  EXPECT_NEAR(options_.hit_probability(),
+              probability_grid_.GetProbability(-3.5, 0.5), 1e-4);
+  EXPECT_NEAR(options_.miss_probability(),
+              probability_grid_.GetProbability(-2.5, 0.5), 1e-4);
+
+  for (int i = 0; i < 1000; ++i) {
+    InsertPointCloud();
+  }
+  EXPECT_NEAR(mapping::kMaxProbability,
+              probability_grid_.GetProbability(-3.5, 0.5), 1e-3);
+  EXPECT_NEAR(mapping::kMinProbability,
+              probability_grid_.GetProbability(-2.5, 0.5), 1e-3);
+}
+
+}  // namespace
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/cartographer/mapping_2d/local_trajectory_builder.cc

@@ -0,0 +1,284 @@
+/*
+ * 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_2d/local_trajectory_builder.h"
+
+#include <limits>
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/sensor/laser.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::LocalTrajectoryBuilderOptions options;
+  options.set_horizontal_laser_min_z(
+      parameter_dictionary->GetDouble("horizontal_laser_min_z"));
+  options.set_horizontal_laser_max_z(
+      parameter_dictionary->GetDouble("horizontal_laser_max_z"));
+  options.set_horizontal_laser_voxel_filter_size(
+      parameter_dictionary->GetDouble("horizontal_laser_voxel_filter_size"));
+  options.set_use_online_correlative_scan_matching(
+      parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
+  *options.mutable_adaptive_voxel_filter_options() =
+      sensor::CreateAdaptiveVoxelFilterOptions(
+          parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
+  *options.mutable_real_time_correlative_scan_matcher_options() =
+      scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
+          parameter_dictionary
+              ->GetDictionary("real_time_correlative_scan_matcher")
+              .get());
+  *options.mutable_ceres_scan_matcher_options() =
+      scan_matching::CreateCeresScanMatcherOptions(
+          parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
+  *options.mutable_motion_filter_options() =
+      mapping_3d::CreateMotionFilterOptions(
+          parameter_dictionary->GetDictionary("motion_filter").get());
+  *options.mutable_pose_tracker_options() =
+      kalman_filter::CreatePoseTrackerOptions(
+          parameter_dictionary->GetDictionary("pose_tracker").get());
+  *options.mutable_submaps_options() = CreateSubmapsOptions(
+      parameter_dictionary->GetDictionary("submaps").get());
+  options.set_expect_imu_data(parameter_dictionary->GetBool("expect_imu_data"));
+  return options;
+}
+
+LocalTrajectoryBuilder::LocalTrajectoryBuilder(
+    const proto::LocalTrajectoryBuilderOptions& options)
+    : options_(options),
+      submaps_(options.submaps_options()),
+      scan_matcher_pose_estimate_(transform::Rigid3d::Identity()),
+      motion_filter_(options_.motion_filter_options()),
+      real_time_correlative_scan_matcher_(
+          options_.real_time_correlative_scan_matcher_options()),
+      ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
+
+LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
+
+const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
+
+Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
+
+kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const {
+  return pose_tracker_.get();
+}
+
+sensor::LaserFan LocalTrajectoryBuilder::BuildProjectedLaserFan(
+    const transform::Rigid3f& tracking_to_tracking_2d,
+    const sensor::LaserFan3D& laser_fan) const {
+  const sensor::LaserFan projected_fan = sensor::ProjectCroppedLaserFan(
+      sensor::TransformLaserFan3D(laser_fan, tracking_to_tracking_2d),
+      Eigen::Vector3f(-std::numeric_limits<float>::infinity(),
+                      -std::numeric_limits<float>::infinity(),
+                      options_.horizontal_laser_min_z()),
+      Eigen::Vector3f(std::numeric_limits<float>::infinity(),
+                      std::numeric_limits<float>::infinity(),
+                      options_.horizontal_laser_max_z()));
+  return sensor::LaserFan{
+      projected_fan.origin,
+      sensor::VoxelFiltered(projected_fan.point_cloud,
+                            options_.horizontal_laser_voxel_filter_size()),
+      sensor::VoxelFiltered(projected_fan.missing_echo_point_cloud,
+                            options_.horizontal_laser_voxel_filter_size())};
+}
+
+void LocalTrajectoryBuilder::ScanMatch(
+    common::Time time, const transform::Rigid3d& pose_prediction,
+    const transform::Rigid3d& tracking_to_tracking_2d,
+    const sensor::LaserFan& laser_fan_in_tracking_2d,
+    transform::Rigid3d* pose_observation,
+    kalman_filter::PoseCovariance* covariance_observation) {
+  const ProbabilityGrid& probability_grid =
+      submaps_.Get(submaps_.matching_index())->probability_grid;
+  transform::Rigid2d pose_prediction_2d =
+      transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse());
+  // The online correlative scan matcher will refine the initial estimate for
+  // the Ceres scan matcher.
+  transform::Rigid2d initial_ceres_pose = pose_prediction_2d;
+  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
+      options_.adaptive_voxel_filter_options());
+  const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d =
+      adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.point_cloud);
+  if (options_.use_online_correlative_scan_matching()) {
+    kalman_filter::Pose2DCovariance unused_covariance_observation;
+    real_time_correlative_scan_matcher_.Match(
+        pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
+        probability_grid, &initial_ceres_pose, &unused_covariance_observation);
+  }
+
+  transform::Rigid2d tracking_2d_to_map;
+  kalman_filter::Pose2DCovariance covariance_observation_2d;
+  ceres::Solver::Summary summary;
+  ceres_scan_matcher_.Match(
+      transform::Project2D(scan_matcher_pose_estimate_ *
+                           tracking_to_tracking_2d.inverse()),
+      initial_ceres_pose, filtered_point_cloud_in_tracking_2d, probability_grid,
+      &tracking_2d_to_map, &covariance_observation_2d, &summary);
+
+  CHECK(pose_tracker_ != nullptr);
+
+  *pose_observation = transform::Embed3D(tracking_2d_to_map);
+  // This covariance is used for non-yaw rotation and the fake height of 0.
+  constexpr double kFakePositionCovariance = 1.;
+  constexpr double kFakeOrientationCovariance = 1.;
+  *covariance_observation =
+      kalman_filter::Embed3D(covariance_observation_2d, kFakePositionCovariance,
+                             kFakeOrientationCovariance);
+  pose_tracker_->AddPoseObservation(
+      time, (*pose_observation) * tracking_to_tracking_2d,
+      *covariance_observation);
+}
+
+std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+LocalTrajectoryBuilder::AddHorizontalLaserFan(
+    const common::Time time, const sensor::LaserFan3D& laser_fan) {
+  // Initialize pose tracker now if we do not ever use an IMU.
+  if (!options_.expect_imu_data()) {
+    InitializePoseTracker(time);
+  }
+
+  if (pose_tracker_ == nullptr) {
+    // Until we've initialized the UKF with our first IMU message, we cannot
+    // compute the orientation of the laser scanner.
+    LOG(INFO) << "PoseTracker not yet initialized.";
+    return nullptr;
+  }
+
+  transform::Rigid3d pose_prediction;
+  kalman_filter::PoseCovariance covariance_prediction;
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_prediction,
+                                                  &covariance_prediction);
+
+  // Computes the rotation without yaw, as defined by GetYaw().
+  const transform::Rigid3d tracking_to_tracking_2d =
+      transform::Rigid3d::Rotation(
+          Eigen::Quaterniond(Eigen::AngleAxisd(
+              -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
+          pose_prediction.rotation());
+
+  const sensor::LaserFan laser_fan_in_tracking_2d =
+      BuildProjectedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan);
+
+  if (laser_fan_in_tracking_2d.point_cloud.empty()) {
+    LOG(WARNING) << "Dropped empty horizontal laser point cloud.";
+    return nullptr;
+  }
+
+  transform::Rigid3d pose_observation;
+  kalman_filter::PoseCovariance covariance_observation;
+  ScanMatch(time, pose_prediction, tracking_to_tracking_2d,
+            laser_fan_in_tracking_2d, &pose_observation,
+            &covariance_observation);
+
+  kalman_filter::PoseCovariance covariance_estimate;
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(
+      time, &scan_matcher_pose_estimate_, &covariance_estimate);
+
+  // Remove the untracked z-component which floats around 0 in the UKF.
+  const auto translation = scan_matcher_pose_estimate_.translation();
+  scan_matcher_pose_estimate_ = transform::Rigid3d(
+      transform::Rigid3d::Vector(translation.x(), translation.y(), 0.),
+      scan_matcher_pose_estimate_.rotation());
+
+  const transform::Rigid3d tracking_2d_to_map =
+      scan_matcher_pose_estimate_ * tracking_to_tracking_2d.inverse();
+  last_pose_estimate_ = {
+      time,
+      {pose_prediction, covariance_prediction},
+      {pose_observation, covariance_observation},
+      {scan_matcher_pose_estimate_, covariance_estimate},
+      scan_matcher_pose_estimate_,
+      sensor::TransformPointCloud(
+          sensor::ToPointCloud(laser_fan_in_tracking_2d.point_cloud),
+          tracking_2d_to_map.cast<float>())};
+
+  const transform::Rigid2d pose_estimate_2d =
+      transform::Project2D(tracking_2d_to_map);
+  if (motion_filter_.IsSimilar(time, transform::Embed3D(pose_estimate_2d))) {
+    return nullptr;
+  }
+
+  const mapping::Submap* const matching_submap =
+      submaps_.Get(submaps_.matching_index());
+  std::vector<const mapping::Submap*> insertion_submaps;
+  for (int insertion_index : submaps_.insertion_indices()) {
+    insertion_submaps.push_back(submaps_.Get(insertion_index));
+  }
+  submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d,
+                                            pose_estimate_2d.cast<float>()));
+
+  return common::make_unique<InsertionResult>(InsertionResult{
+      time, &submaps_, matching_submap, insertion_submaps,
+      tracking_to_tracking_2d, tracking_2d_to_map, laser_fan_in_tracking_2d,
+      pose_estimate_2d, covariance_estimate});
+}
+
+const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&
+LocalTrajectoryBuilder::pose_estimate() const {
+  return last_pose_estimate_;
+}
+
+void LocalTrajectoryBuilder::AddImuData(
+    const common::Time time, const Eigen::Vector3d& linear_acceleration,
+    const Eigen::Vector3d& angular_velocity) {
+  CHECK(options_.expect_imu_data())
+      << "An IMU packet was added, but the IMU is not in the "
+         "sensor_configuration.";
+
+  InitializePoseTracker(time);
+  pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
+  pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
+
+  transform::Rigid3d pose_estimate;
+  kalman_filter::PoseCovariance unused_covariance_estimate;
+  pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_estimate,
+                                                  &unused_covariance_estimate);
+
+  // Log a warning if the backpack inclination exceeds 20 degrees. In these
+  // cases, it's very likely that 2D SLAM will fail.
+  const Eigen::Vector3d gravity_direction =
+      Eigen::Quaterniond(pose_estimate.rotation()) * Eigen::Vector3d::UnitZ();
+  const double inclination = std::acos(gravity_direction.z());
+  constexpr double kMaxInclination = common::DegToRad(20.);
+  LOG_IF_EVERY_N(WARNING, inclination > kMaxInclination, 1000)
+      << "Max inclination exceeded: " << common::RadToDeg(inclination) << " > "
+      << common::RadToDeg(kMaxInclination);
+}
+
+void LocalTrajectoryBuilder::AddOdometerPose(
+    const common::Time time, const transform::Rigid3d& pose,
+    const kalman_filter::PoseCovariance& covariance) {
+  if (pose_tracker_ == nullptr) {
+    // Until we've initialized the UKF with our first IMU message, we cannot
+    // process odometry poses.
+    LOG_EVERY_N(INFO, 100) << "PoseTracker not yet initialized.";
+  } else {
+    pose_tracker_->AddOdometerPoseObservation(time, pose, covariance);
+  }
+}
+
+void LocalTrajectoryBuilder::InitializePoseTracker(const common::Time time) {
+  if (pose_tracker_ == nullptr) {
+    pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
+        options_.pose_tracker_options(),
+        kalman_filter::PoseTracker::ModelFunction::k2D, time);
+  }
+}
+
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/cartographer/mapping_2d/local_trajectory_builder.h

@@ -0,0 +1,113 @@
+/*
+ * 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_2D_LOCAL_TRAJECTORY_BUILDER_H_
+#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_
+
+#include <memory>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/time.h"
+#include "cartographer/kalman_filter/pose_tracker.h"
+#include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h"
+#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
+#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
+#include "cartographer/mapping_2d/submaps.h"
+#include "cartographer/mapping_3d/motion_filter.h"
+#include "cartographer/sensor/configuration.h"
+#include "cartographer/sensor/voxel_filter.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
+    common::LuaParameterDictionary* parameter_dictionary);
+
+// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
+// closure.
+class LocalTrajectoryBuilder {
+ public:
+  struct InsertionResult {
+    common::Time time;
+    const mapping::Submaps* submaps;
+    const mapping::Submap* matching_submap;
+    std::vector<const mapping::Submap*> insertion_submaps;
+    transform::Rigid3d tracking_to_tracking_2d;
+    transform::Rigid3d tracking_2d_to_map;
+    sensor::LaserFan laser_fan_in_tracking_2d;
+    transform::Rigid2d pose_estimate_2d;
+    kalman_filter::PoseCovariance covariance_estimate;
+  };
+
+  explicit LocalTrajectoryBuilder(
+      const proto::LocalTrajectoryBuilderOptions& options);
+  ~LocalTrajectoryBuilder();
+
+  LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
+  LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
+
+  const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
+      const;
+  std::unique_ptr<InsertionResult> AddHorizontalLaserFan(
+      common::Time, const sensor::LaserFan3D& laser_fan);
+  void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
+                  const Eigen::Vector3d& angular_velocity);
+  void AddOdometerPose(common::Time time, const transform::Rigid3d& pose,
+                       const kalman_filter::PoseCovariance& covariance);
+
+  const Submaps* submaps() const;
+  Submaps* submaps();
+  kalman_filter::PoseTracker* pose_tracker() const;
+
+ private:
+  // Transforms 'laser_scan', projects it onto the ground plane,
+  // crops and voxel filters.
+  sensor::LaserFan BuildProjectedLaserFan(
+      const transform::Rigid3f& tracking_to_tracking_2d,
+      const sensor::LaserFan3D& laser_fan) const;
+
+  // Scan match 'laser_fan_in_tracking_2d' and fill in the
+  // 'pose_observation' and 'covariance_observation' with the result.
+  void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
+                 const transform::Rigid3d& tracking_to_tracking_2d,
+                 const sensor::LaserFan& laser_fan_in_tracking_2d,
+                 transform::Rigid3d* pose_observation,
+                 kalman_filter::PoseCovariance* covariance_observation);
+
+  // Lazily constructs a PoseTracker.
+  void InitializePoseTracker(common::Time time);
+
+  const proto::LocalTrajectoryBuilderOptions options_;
+  Submaps submaps_;
+  mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_;
+
+  // Pose of the last computed scan match.
+  transform::Rigid3d scan_matcher_pose_estimate_;
+
+  mapping_3d::MotionFilter motion_filter_;
+  scan_matching::RealTimeCorrelativeScanMatcher
+      real_time_correlative_scan_matcher_;
+  scan_matching::CeresScanMatcher ceres_scan_matcher_;
+
+  std::unique_ptr<kalman_filter::PoseTracker> pose_tracker_;
+};
+
+}  // namespace mapping_2d
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_

cartographer/cartographer/mapping_2d/map_limits.h

@@ -0,0 +1,193 @@
+/*
+ * 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_2D_MAP_LIMITS_H_
+#define CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_
+
+#include <utility>
+#include <vector>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/common/math.h"
+#include "cartographer/mapping/trajectory_node.h"
+#include "cartographer/mapping_2d/xy_index.h"
+#include "cartographer/sensor/laser.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+// Defines the limits of a grid map. This class must remain inlined for
+// performance reasons.
+class MapLimits {
+ public:
+  MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits) {
+    SetLimits(resolution, edge_limits);
+  }
+
+  MapLimits(const double resolution, const double max_x, const double max_y,
+            const CellLimits& cell_limits) {
+    SetLimits(resolution, max_x, max_y, cell_limits);
+  }
+
+  MapLimits(const double resolution, const double center_x,
+            const double center_y) {
+    SetLimits(resolution, center_x + 100 * resolution,
+              center_y + 100 * resolution, CellLimits(200, 200));
+  }
+
+  // Returns the cell size in meters. All cells are square and the resolution is
+  // the length of one side.
+  double resolution() const { return resolution_; }
+
+  // Returns the limits of the grid from edge to edge in meters.
+  const Eigen::AlignedBox2d& edge_limits() const { return edge_limits_; }
+
+  // Returns the limits of the grid between the centers of the edge cells in
+  // meters.
+  const Eigen::AlignedBox2d& centered_limits() const {
+    return centered_limits_;
+  }
+
+  // Returns the limits of the grid in number of cells.
+  const CellLimits& cell_limits() const { return cell_limits_; }
+
+  // Returns the index of the cell containing the point ('x', 'y') which may be
+  // outside the map, i.e., negative or too large indices that will return
+  // false for Contains().
+  Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x,
+                                                 const double y) const {
+    return mapping_2d::GetXYIndexOfCellContainingPoint(
+        x, y, centered_limits_.max().x(), centered_limits_.max().y(),
+        resolution_);
+  }
+
+  // Returns true of the ProbabilityGrid contains 'xy_index'.
+  bool Contains(const Eigen::Array2i& xy_index) const {
+    return (Eigen::Array2i(0, 0) <= xy_index).all() &&
+           (xy_index <
+            Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
+               .all();
+  }
+
+  // Computes MapLimits that contain the origin, and all laser rays (both
+  // returns and missing echoes) in the 'trajectory'.
+  static MapLimits ComputeMapLimits(
+      const double resolution,
+      const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
+    Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes);
+    // Add some padding to ensure all rays are still contained in the map after
+    // discretization.
+    const float kPadding = 3.f * resolution;
+    bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
+    bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
+    return MapLimits(resolution, bounding_box.cast<double>());
+  }
+
+  static Eigen::AlignedBox2f ComputeMapBoundingBox(
+      const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
+    Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
+    for (const auto& node : trajectory_nodes) {
+      const auto& data = *node.constant_data;
+      if (!data.laser_fan_3d.returns.empty()) {
+        const sensor::LaserFan3D laser_fan_3d = sensor::TransformLaserFan3D(
+            Decompress(data.laser_fan_3d), node.pose.cast<float>());
+        bounding_box.extend(laser_fan_3d.origin.head<2>());
+        for (const Eigen::Vector3f& hit : laser_fan_3d.returns) {
+          bounding_box.extend(hit.head<2>());
+        }
+      } else {
+        const sensor::LaserFan laser_fan = sensor::TransformLaserFan(
+            data.laser_fan, transform::Project2D(node.pose).cast<float>());
+        bounding_box.extend(laser_fan.origin);
+        for (const Eigen::Vector2f& hit : laser_fan.point_cloud) {
+          bounding_box.extend(hit);
+        }
+        for (const Eigen::Vector2f& miss : laser_fan.missing_echo_point_cloud) {
+          bounding_box.extend(miss);
+        }
+      }
+    }
+    return bounding_box;
+  }
+
+ private:
+  // Sets the cell size to the specified resolution in meters and the limits of
+  // the grid to the specified bounding box in meters.
+  void SetLimits(double resolution, const Eigen::AlignedBox2d& limits) {
+    CHECK(!limits.isEmpty());
+    const int num_x_cells =
+        common::RoundToInt((mapping_2d::Center(limits.max().y(), resolution) -
+                            mapping_2d::Center(limits.min().y(), resolution)) /
+                           resolution) +
+        1;
+    const int num_y_cells =
+        common::RoundToInt((mapping_2d::Center(limits.max().x(), resolution) -
+                            mapping_2d::Center(limits.min().x(), resolution)) /
+                           resolution) +
+        1;
+    SetLimits(resolution, limits.max().x(), limits.max().y(),
+              CellLimits(num_x_cells, num_y_cells));
+  }
+
+  // Sets the cell size to the specified resolution in meters and the limits of
+  // the grid to the specified bounding box.
+  //
+  // Note that implementing this in terms of the previous SetLimits method
+  // results in unnecessary (and expensive?) calls to common::RoundToInt.
+  //
+  // TODO(whess): Measure whether it really is still too slow. Otherwise,
+  // simplify.
+  void SetLimits(double resolution, double max_x, double max_y,
+                 const CellLimits& limits) {
+    CHECK_GT(resolution, 0.);
+    CHECK_GT(limits.num_x_cells, 0.);
+    CHECK_GT(limits.num_y_cells, 0.);
+
+    resolution_ = resolution;
+    cell_limits_ = limits;
+    centered_limits_.max().x() = Center(max_x, resolution);
+    centered_limits_.max().y() = Center(max_y, resolution);
+    centered_limits_.min().x() = centered_limits_.max().x() -
+                                 resolution_ * (cell_limits_.num_y_cells - 1);
+    centered_limits_.min().y() = centered_limits_.max().y() -
+                                 resolution_ * (cell_limits_.num_x_cells - 1);
+    UpdateEdgeLimits();
+  }
+
+  // Updates the edge limits from the previously calculated centered limits.
+  void UpdateEdgeLimits() {
+    const double half_resolution = resolution_ / 2.;
+    edge_limits_.min().x() = centered_limits_.min().x() - half_resolution;
+    edge_limits_.min().y() = centered_limits_.min().y() - half_resolution;
+    edge_limits_.max().x() = centered_limits_.max().x() + half_resolution;
+    edge_limits_.max().y() = centered_limits_.max().y() + half_resolution;
+  }
+
+  double resolution_;
+  Eigen::AlignedBox2d edge_limits_;
+  Eigen::AlignedBox2d centered_limits_;
+  CellLimits cell_limits_;
+};
+
+}  // namespace mapping_2d
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_

cartographer/cartographer/mapping_2d/map_limits_test.cc

@@ -0,0 +1,66 @@
+/*
+ * 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_2d/map_limits.h"
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping_2d {
+namespace {
+
+TEST(MapLimits, SetEdgeLimits) {
+  const MapLimits limits(2., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 1.5),
+                                                 Eigen::Vector2d(0.5, 5.5)));
+
+  const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
+  EXPECT_EQ(-2., edge_limits.min().x());
+  EXPECT_EQ(0., edge_limits.min().y());
+  EXPECT_EQ(2., edge_limits.max().x());
+  EXPECT_EQ(6., edge_limits.max().y());
+
+  const CellLimits& cell_limits = limits.cell_limits();
+  EXPECT_EQ(3, cell_limits.num_x_cells);
+  EXPECT_EQ(2, cell_limits.num_y_cells);
+}
+
+TEST(MapLimits, SetCellLimits) {
+  const MapLimits limits(3., 1.5, -0.5, CellLimits(2, 3));
+
+  const CellLimits& cell_limits = limits.cell_limits();
+  EXPECT_EQ(2, cell_limits.num_x_cells);
+  EXPECT_EQ(3, cell_limits.num_y_cells);
+
+  const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
+  EXPECT_EQ(-6., edge_limits.min().x());
+  EXPECT_EQ(-6., edge_limits.min().y());
+  EXPECT_EQ(3., edge_limits.max().x());
+  EXPECT_EQ(0., edge_limits.max().y());
+}
+
+TEST(MapLimits, GetCenteredLimits) {
+  const MapLimits limits(2., -0.5, 1.5, CellLimits(3, 2));
+
+  const Eigen::AlignedBox2d& centered_limits = limits.centered_limits();
+  EXPECT_EQ(-3, centered_limits.min().x());
+  EXPECT_EQ(-3., centered_limits.min().y());
+  EXPECT_EQ(-1., centered_limits.max().x());
+  EXPECT_EQ(1., centered_limits.max().y());
+}
+
+}  // namespace
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/cartographer/mapping_2d/probability_grid.h

@@ -0,0 +1,197 @@
+/*
+ * 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_2D_PROBABILITY_GRID_H_
+#define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "cartographer/common/math.h"
+#include "cartographer/common/port.h"
+#include "cartographer/mapping/probability_values.h"
+#include "cartographer/mapping_2d/map_limits.h"
+#include "cartographer/mapping_2d/xy_index.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+// Represents a 2D grid of probabilities.
+class ProbabilityGrid {
+ public:
+  explicit ProbabilityGrid(const MapLimits& limits)
+      : limits_(limits),
+        cells_(limits_.cell_limits().num_x_cells *
+                   limits_.cell_limits().num_y_cells,
+               mapping::kUnknownProbabilityValue),
+        max_x_(0),
+        max_y_(0),
+        min_x_(limits_.cell_limits().num_x_cells - 1),
+        min_y_(limits_.cell_limits().num_y_cells - 1) {}
+
+  // Returns the limits of this ProbabilityGrid.
+  const MapLimits& limits() const { return limits_; }
+
+  // Starts the next update sequence.
+  void StartUpdate() {
+    while (!update_indices_.empty()) {
+      DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
+      cells_[update_indices_.back()] -= mapping::kUpdateMarker;
+      update_indices_.pop_back();
+    }
+  }
+
+  // Sets the probability of the cell at 'xy_index' to the given 'probability'.
+  // Only allowed if the cell was unknown before.
+  void SetProbability(const Eigen::Array2i& xy_index, const float probability) {
+    uint16& cell = cells_[GetIndexOfCell(xy_index)];
+    CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
+    cell = mapping::ProbabilityToValue(probability);
+    UpdateBounds(xy_index);
+  }
+
+  // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
+  // to the probability of the cell at 'xy_index' if the cell has not already
+  // been updated. Multiple updates of the same cell will be ignored until
+  // StartUpdate() is called. Returns true if the cell was updated.
+  //
+  // If this is the first call to ApplyOdds() for the specified cell, its value
+  // will be set to probability corresponding to 'odds'.
+  bool ApplyLookupTable(const Eigen::Array2i& xy_index,
+                        const std::vector<uint16>& table) {
+    DCHECK_EQ(table.size(), mapping::kUpdateMarker);
+    const int cell_index = GetIndexOfCell(xy_index);
+    uint16& cell = cells_[cell_index];
+    if (cell >= mapping::kUpdateMarker) {
+      return false;
+    }
+    update_indices_.push_back(cell_index);
+    cell = table[cell];
+    DCHECK_GE(cell, mapping::kUpdateMarker);
+    UpdateBounds(xy_index);
+    return true;
+  }
+
+  // Returns the probability of the cell with 'xy_index'.
+  float GetProbability(const Eigen::Array2i& xy_index) const {
+    if (limits_.Contains(xy_index)) {
+      return mapping::ValueToProbability(cells_[GetIndexOfCell(xy_index)]);
+    }
+    return mapping::kMinProbability;
+  }
+
+  // Returns the probability of the cell containing the point ('x', 'y').
+  float GetProbability(const double x, const double y) const {
+    return GetProbability(limits_.GetXYIndexOfCellContainingPoint(x, y));
+  }
+
+  // Returns true if the probability at the specified index is known.
+  bool IsKnown(const Eigen::Array2i& xy_index) const {
+    return limits_.Contains(xy_index) &&
+           cells_[GetIndexOfCell(xy_index)] !=
+               mapping::kUnknownProbabilityValue;
+  }
+
+  // Returns the center of the cell at 'xy_index'.
+  void GetCenterOfCellWithXYIndex(const Eigen::Array2i& xy_index,
+                                  double* const x, double* const y) const {
+    *CHECK_NOTNULL(x) = limits_.edge_limits().max().x() -
+                        (xy_index.y() + 0.5) * limits_.resolution();
+    *CHECK_NOTNULL(y) = limits_.edge_limits().max().y() -
+                        (xy_index.x() + 0.5) * limits_.resolution();
+  }
+
+  // Fills in 'offset' and 'limits' to define a subregion of that contains all
+  // known cells.
+  void ComputeCroppedLimits(Eigen::Array2i* const offset,
+                            CellLimits* const limits) const {
+    *offset = Eigen::Array2i(min_x_, min_y_);
+    *limits = CellLimits(std::max(max_x_, min_x_) - min_x_ + 1,
+                         std::max(max_y_, min_y_) - min_y_ + 1);
+  }
+
+  // Grows the map as necessary to include 'x' and 'y'. This changes the meaning
+  // of these coordinates going forward. This method must be called immediately
+  // after 'StartUpdate', before any calls to 'ApplyLookupTable'.
+  void GrowLimits(const double x, const double y) {
+    CHECK(update_indices_.empty());
+    while (!limits_.Contains(limits_.GetXYIndexOfCellContainingPoint(x, y))) {
+      const int x_offset = limits_.cell_limits().num_x_cells / 2;
+      const int y_offset = limits_.cell_limits().num_y_cells / 2;
+      const MapLimits new_limits(
+          limits_.resolution(),
+          limits_.centered_limits().max().x() + y_offset * limits_.resolution(),
+          limits_.centered_limits().max().y() + x_offset * limits_.resolution(),
+          CellLimits(2 * limits_.cell_limits().num_x_cells,
+                     2 * limits_.cell_limits().num_y_cells));
+      const int stride = new_limits.cell_limits().num_x_cells;
+      const int offset = x_offset + stride * y_offset;
+      const int new_size = new_limits.cell_limits().num_x_cells *
+                           new_limits.cell_limits().num_y_cells;
+      std::vector<uint16> new_cells(new_size,
+                                    mapping::kUnknownProbabilityValue);
+      for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
+        for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
+          new_cells[offset + j + i * stride] =
+              cells_[j + i * limits_.cell_limits().num_x_cells];
+        }
+      }
+      cells_ = new_cells;
+      limits_ = new_limits;
+      min_x_ += x_offset;
+      min_y_ += y_offset;
+      max_x_ += x_offset;
+      max_y_ += y_offset;
+    }
+  }
+
+ private:
+  // Returns the index of the cell at 'xy_index'.
+  int GetIndexOfCell(const Eigen::Array2i& xy_index) const {
+    CHECK(limits_.Contains(xy_index)) << xy_index;
+    return mapping_2d::GetIndexOfCell(xy_index,
+                                      limits_.cell_limits().num_x_cells);
+  }
+
+  void UpdateBounds(const Eigen::Array2i& xy_index) {
+    min_x_ = std::min(min_x_, xy_index.x());
+    min_y_ = std::min(min_y_, xy_index.y());
+    max_x_ = std::max(max_x_, xy_index.x());
+    max_y_ = std::max(max_y_, xy_index.y());
+  }
+
+  MapLimits limits_;
+  std::vector<uint16> cells_;  // Highest bit is update marker.
+  std::vector<int> update_indices_;
+
+  // Minimum and maximum cell coordinates of know cells to efficiently compute
+  // cropping limits.
+  int max_x_;
+  int max_y_;
+  int min_x_;
+  int min_y_;
+};
+
+}  // namespace mapping_2d
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_

cartographer/cartographer/mapping_2d/probability_grid_test.cc

@@ -0,0 +1,190 @@
+/*
+ * 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_2d/probability_grid.h"
+
+#include <random>
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping_2d {
+namespace {
+
+TEST(ProbabilityGridTest, ApplyOdds) {
+  ProbabilityGrid probability_grid(MapLimits(1., 0.5, 0.5, CellLimits(2, 2)));
+  const MapLimits& limits = probability_grid.limits();
+
+  EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
+  EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 1)));
+  EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 0)));
+  EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 1)));
+  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 0)));
+  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 1)));
+  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 0)));
+  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 1)));
+
+  probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5);
+
+  probability_grid.StartUpdate();
+  probability_grid.ApplyLookupTable(
+      Eigen::Array2i(1, 0),
+      mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
+  EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5);
+
+  probability_grid.StartUpdate();
+  probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5);
+
+  probability_grid.StartUpdate();
+  probability_grid.ApplyLookupTable(
+      Eigen::Array2i(0, 1),
+      mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1)));
+  EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5);
+
+  // Tests adding odds to an unknown cell.
+  probability_grid.StartUpdate();
+  probability_grid.ApplyLookupTable(
+      Eigen::Array2i(1, 1),
+      mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42)));
+  EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
+              1e-4);
+
+  // Tests that further updates are ignored if StartUpdate() isn't called.
+  probability_grid.ApplyLookupTable(
+      Eigen::Array2i(1, 1),
+      mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
+  EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
+              1e-4);
+  probability_grid.StartUpdate();
+  probability_grid.ApplyLookupTable(
+      Eigen::Array2i(1, 1),
+      mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
+  EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42);
+}
+
+TEST(ProbabilityGridTest, GetProbability) {
+  ProbabilityGrid probability_grid(
+      MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 0.5),
+                                        Eigen::Vector2d(0.5, 1.5))));
+
+  const MapLimits& limits = probability_grid.limits();
+  const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
+  EXPECT_EQ(-1., edge_limits.min().x());
+  EXPECT_EQ(0., edge_limits.min().y());
+  EXPECT_EQ(1., edge_limits.max().x());
+  EXPECT_EQ(2., edge_limits.max().y());
+
+  const CellLimits& cell_limits = limits.cell_limits();
+  ASSERT_EQ(2, cell_limits.num_x_cells);
+  ASSERT_EQ(2, cell_limits.num_y_cells);
+
+  probability_grid.StartUpdate();
+  probability_grid.SetProbability(
+      limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5),
+      mapping::kMaxProbability);
+  EXPECT_NEAR(probability_grid.GetProbability(-0.5, 0.5),
+              mapping::kMaxProbability, 1e-6);
+  for (const Eigen::Array2i& xy_index :
+       {limits.GetXYIndexOfCellContainingPoint(-0.5, 1.5),
+        limits.GetXYIndexOfCellContainingPoint(0.5, 0.5),
+        limits.GetXYIndexOfCellContainingPoint(0.5, 1.5)}) {
+    EXPECT_TRUE(limits.Contains(xy_index));
+    EXPECT_FALSE(probability_grid.IsKnown(xy_index));
+  }
+}
+
+TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
+  ProbabilityGrid probability_grid(MapLimits(2., 7, 13, CellLimits(14, 8)));
+
+  const MapLimits& limits = probability_grid.limits();
+  const CellLimits& cell_limits = limits.cell_limits();
+  ASSERT_EQ(14, cell_limits.num_x_cells);
+  ASSERT_EQ(8, cell_limits.num_y_cells);
+  EXPECT_TRUE(
+      (Eigen::Array2i(0, 0) == limits.GetXYIndexOfCellContainingPoint(7, 13))
+          .all());
+  EXPECT_TRUE(
+      (Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13))
+          .all());
+  EXPECT_TRUE(
+      (Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13))
+          .all());
+  EXPECT_TRUE(
+      (Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13))
+          .all());
+
+  // Check around the origin.
+  EXPECT_TRUE(
+      (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, 0.5))
+          .all());
+  EXPECT_TRUE(
+      (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(1.5, 1.5))
+          .all());
+  EXPECT_TRUE((Eigen::Array2i(7, 3) ==
+               limits.GetXYIndexOfCellContainingPoint(0.5, -0.5))
+                  .all());
+  EXPECT_TRUE((Eigen::Array2i(6, 4) ==
+               limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5))
+                  .all());
+  EXPECT_TRUE((Eigen::Array2i(7, 4) ==
+               limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5))
+                  .all());
+}
+
+TEST(ProbabilityGridTest, GetCenterOfCell) {
+  ProbabilityGrid probability_grid(MapLimits(
+      2.,
+      Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.), Eigen::Vector2d(-0., 6.))));
+
+  // Limits are on the edge of a cell. Make sure that the grid grows (in
+  // positive direction).
+  const MapLimits& limits = probability_grid.limits();
+  const CellLimits& cell_limits = limits.cell_limits();
+  EXPECT_EQ(4, cell_limits.num_x_cells);
+  EXPECT_EQ(3, cell_limits.num_y_cells);
+
+  const Eigen::Array2i xy_index(3, 2);
+  double x, y;
+  probability_grid.GetCenterOfCellWithXYIndex(xy_index, &x, &y);
+  EXPECT_NEAR(-3., x, 1e-6);
+  EXPECT_NEAR(1., y, 1e-6);
+  EXPECT_TRUE((xy_index == limits.GetXYIndexOfCellContainingPoint(x, y)).all());
+}
+
+TEST(ProbabilityGridTest, CorrectCropping) {
+  // Create a probability grid with random values.
+  std::mt19937 rng(42);
+  std::uniform_real_distribution<float> value_distribution(
+      mapping::kMinProbability, mapping::kMaxProbability);
+  ProbabilityGrid probability_grid(
+      MapLimits(0.05, 10., 10., CellLimits(400, 400)));
+  probability_grid.StartUpdate();
+  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
+           probability_grid.limits().cell_limits(), Eigen::Array2i(100, 100),
+           Eigen::Array2i(299, 299))) {
+    probability_grid.SetProbability(xy_index, value_distribution(rng));
+  }
+  Eigen::Array2i offset;
+  CellLimits limits;
+  probability_grid.ComputeCroppedLimits(&offset, &limits);
+  EXPECT_TRUE((offset == Eigen::Array2i(100, 100)).all());
+  EXPECT_EQ(limits.num_x_cells, 200);
+  EXPECT_EQ(limits.num_y_cells, 200);
+}
+
+}  // namespace
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/cartographer/mapping_2d/proto/CMakeLists.txt

@@ -0,0 +1,37 @@
+# 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.
+
+google_proto_library(mapping_2d_proto_laser_fan_inserter_options
+  SRCS
+    laser_fan_inserter_options.proto
+)
+
+google_proto_library(mapping_2d_proto_local_trajectory_builder_options
+  SRCS
+    local_trajectory_builder_options.proto
+  DEPENDS
+    kalman_filter_proto_pose_tracker_options
+    mapping_2d_proto_submaps_options
+    mapping_2d_scan_matching_proto_ceres_scan_matcher_options
+    mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options
+    mapping_3d_proto_motion_filter_options
+    sensor_proto_adaptive_voxel_filter_options
+)
+
+google_proto_library(mapping_2d_proto_submaps_options
+  SRCS
+    submaps_options.proto
+  DEPENDS
+    mapping_2d_proto_laser_fan_inserter_options
+)

cartographer/cartographer/mapping_2d/proto/laser_fan_inserter_options.proto

@@ -0,0 +1,31 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping_2d.proto;
+
+message LaserFanInserterOptions {
+  // Probability change for a hit (this will be converted to odds and therefore
+  // must be greater than 0.5).
+  optional double hit_probability = 1;
+
+  // Probability change for a miss (this will be converted to odds and therefore
+  // must be less than 0.5).
+  optional double miss_probability = 2;
+
+  // If 'false', free space will not change the probabilities in the occupancy
+  // grid.
+  optional bool insert_free_space = 3;
+}

cartographer/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto

@@ -0,0 +1,53 @@
+// 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.
+
+syntax = "proto2";
+
+package cartographer.mapping_2d.proto;
+
+import "cartographer/kalman_filter/proto/pose_tracker_options.proto";
+import "cartographer/mapping_3d/proto/motion_filter_options.proto";
+import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
+import "cartographer/mapping_2d/proto/submaps_options.proto";
+import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
+import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
+
+message LocalTrajectoryBuilderOptions {
+  // Cropping parameters for the horizontal laser fan.
+  optional float horizontal_laser_min_z = 1;
+  optional float horizontal_laser_max_z = 2;
+
+  // Voxel filter that gets applied to the horizontal laser immediately after
+  // cropping.
+  optional float horizontal_laser_voxel_filter_size = 3;
+
+  // Whether to solve the online scan matching first using the correlative scan
+  // matcher to generate a good starting point for Ceres.
+  optional bool use_online_correlative_scan_matching = 5;
+
+  // Voxel filter used to compute a sparser point cloud for matching.
+  optional sensor.proto.AdaptiveVoxelFilterOptions
+      adaptive_voxel_filter_options = 6;
+
+  optional scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
+      real_time_correlative_scan_matcher_options = 7;
+  optional scan_matching.proto.CeresScanMatcherOptions
+      ceres_scan_matcher_options = 8;
+  optional mapping_3d.proto.MotionFilterOptions motion_filter_options = 13;
+  optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 10;
+  optional mapping_2d.proto.SubmapsOptions submaps_options = 11;
+
+  // True if an IMU is configured in the sensor data.
+  optional bool expect_imu_data = 12;
+}

cartographer/cartographer/mapping_2d/proto/submaps_options.proto

@@ -0,0 +1,37 @@
+// 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.
+
+syntax = "proto2";
+
+import "cartographer/mapping_2d/proto/laser_fan_inserter_options.proto";
+
+package cartographer.mapping_2d.proto;
+
+message SubmapsOptions {
+  // Resolution of the map in meters.
+  optional double resolution = 1;
+
+  // Half the width/height of each submap, its "radius".
+  optional double half_length = 2;
+
+  // Number of scans before adding a new submap. Each submap will get twice the
+  // number of scans inserted: First for initialization without being matched
+  // against, then while being matched.
+  optional int32 num_laser_fans = 3;
+
+  // If enabled, submap%d.png images are written for debugging.
+  optional bool output_debug_images = 4;
+
+  optional LaserFanInserterOptions laser_fan_inserter_options = 5;
+}

cartographer/cartographer/mapping_2d/ray_casting.cc

@@ -0,0 +1,187 @@
+/*
+ * 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_2d/ray_casting.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+namespace {
+
+// Factor for subpixel accuracy of start and end point.
+constexpr int kSubpixelScale = 1000;
+
+// We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin'
+// and 'end' are coordinates at subpixel precision. We compute all pixels in
+// which some part of the line segment connecting 'begin' and 'end' lies.
+void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
+             const std::function<void(const Eigen::Array2i&)>& visitor) {
+  // For simplicity, we order 'begin' and 'end' by their x coordinate.
+  if (begin.x() > end.x()) {
+    CastRay(end, begin, visitor);
+    return;
+  }
+
+  CHECK_GE(begin.x(), 0);
+  CHECK_GE(begin.y(), 0);
+  CHECK_GE(end.y(), 0);
+
+  // Special case: We have to draw a vertical line in full pixels, as 'begin'
+  // and 'end' have the same full pixel x coordinate.
+  if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
+    Eigen::Array2i current(begin.x() / kSubpixelScale,
+                           std::min(begin.y(), end.y()) / kSubpixelScale);
+    const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
+    for (; current.y() <= end_y; ++current.y()) {
+      visitor(current);
+    }
+    return;
+  }
+
+  const int64 dx = end.x() - begin.x();
+  const int64 dy = end.y() - begin.y();
+  const int64 denominator = 2 * kSubpixelScale * dx;
+
+  // The current full pixel coordinates. We begin at 'begin'.
+  Eigen::Array2i current = begin / kSubpixelScale;
+
+  // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
+  // the denominator.
+  // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
+  // | | | |
+  // +-+-+-+
+  // | | | |
+  // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
+  // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
+  // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)
+
+  // The center of the subpixel part of 'begin.y()' assuming the
+  // 'denominator', i.e., sub_y / denominator is in (0, 1).
+  int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;
+
+  // The distance from the from 'begin' to the right pixel border, to be divided
+  // by 2 * 'kSubpixelScale'.
+  const int first_pixel =
+      2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
+  // The same from the left pixel border to 'end'.
+  const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;
+
+  // The full pixel x coordinate of 'end'.
+  const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;
+
+  // Move from 'begin' to the next pixel border to the right.
+  sub_y += dy * first_pixel;
+  if (dy > 0) {
+    while (true) {
+      visitor(current);
+      while (sub_y > denominator) {
+        sub_y -= denominator;
+        ++current.y();
+        visitor(current);
+      }
+      ++current.x();
+      if (sub_y == denominator) {
+        sub_y -= denominator;
+        ++current.y();
+      }
+      if (current.x() == end_x) {
+        break;
+      }
+      // Move from one pixel border to the next.
+      sub_y += dy * 2 * kSubpixelScale;
+    }
+    // Move from the pixel border on the right to 'end'.
+    sub_y += dy * last_pixel;
+    visitor(current);
+    while (sub_y > denominator) {
+      sub_y -= denominator;
+      ++current.y();
+      visitor(current);
+    }
+    CHECK_NE(sub_y, denominator);
+    CHECK_EQ(current.y(), end.y() / kSubpixelScale);
+    return;
+  }
+
+  // Same for lines non-ascending in y coordinates.
+  while (true) {
+    visitor(current);
+    while (sub_y < 0) {
+      sub_y += denominator;
+      --current.y();
+      visitor(current);
+    }
+    ++current.x();
+    if (sub_y == 0) {
+      sub_y += denominator;
+      --current.y();
+    }
+    if (current.x() == end_x) {
+      break;
+    }
+    sub_y += dy * 2 * kSubpixelScale;
+  }
+  sub_y += dy * last_pixel;
+  visitor(current);
+  while (sub_y < 0) {
+    sub_y += denominator;
+    --current.y();
+    visitor(current);
+  }
+  CHECK_NE(sub_y, 0);
+  CHECK_EQ(current.y(), end.y() / kSubpixelScale);
+}
+
+}  // namespace
+
+void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
+              const std::function<void(const Eigen::Array2i&)>& hit_visitor,
+              const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
+  const double resolution = limits.resolution() / kSubpixelScale;
+  const Eigen::Vector2d superscaled_centered_max =
+      limits.edge_limits().max() - resolution / 2. * Eigen::Vector2d::Ones();
+  const Eigen::Array2i begin = GetXYIndexOfCellContainingPoint(
+      laser_fan.origin.x(), laser_fan.origin.y(), superscaled_centered_max.x(),
+      superscaled_centered_max.y(), resolution);
+
+  // Compute and add the end points.
+  std::vector<Eigen::Array2i> ends;
+  ends.reserve(laser_fan.point_cloud.size());
+  for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
+    ends.push_back(GetXYIndexOfCellContainingPoint(
+        laser_return.x(), laser_return.y(), superscaled_centered_max.x(),
+        superscaled_centered_max.y(), resolution));
+    hit_visitor(ends.back() / kSubpixelScale);
+  }
+
+  // Now add the misses.
+  for (const Eigen::Array2i& end : ends) {
+    CastRay(begin, end, miss_visitor);
+  }
+
+  // Finally, compute and add empty rays based on missing echos in the scan.
+  for (const Eigen::Vector2f& missing_echo :
+       laser_fan.missing_echo_point_cloud) {
+    CastRay(begin, GetXYIndexOfCellContainingPoint(
+                       missing_echo.x(), missing_echo.y(),
+                       superscaled_centered_max.x(),
+                       superscaled_centered_max.y(), resolution),
+            miss_visitor);
+  }
+}
+
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/cartographer/mapping_2d/ray_casting.h

@@ -0,0 +1,40 @@
+/*
+ * 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_2D_RAY_CASTING_H_
+#define CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_
+
+#include <functional>
+
+#include "cartographer/mapping_2d/map_limits.h"
+#include "cartographer/mapping_2d/xy_index.h"
+#include "cartographer/sensor/laser.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace mapping_2d {
+
+// For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the
+// appropriate cells. Hits are handled before misses.
+void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
+              const std::function<void(const Eigen::Array2i&)>& hit_visitor,
+              const std::function<void(const Eigen::Array2i&)>& miss_visitor);
+
+}  // namespace mapping_2d
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_

cartographer/cartographer/mapping_2d/scan_matching/CMakeLists.txt

@@ -0,0 +1,170 @@
+# 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.
+
+add_subdirectory("proto")
+
+google_library(mapping_2d_scan_matching_ceres_scan_matcher
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    ceres_scan_matcher.cc
+  HDRS
+    ceres_scan_matcher.h
+  DEPENDS
+    common_ceres_solver_options
+    common_lua_parameter_dictionary
+    kalman_filter_pose_tracker
+    mapping_2d_probability_grid
+    mapping_2d_scan_matching_occupied_space_cost_functor
+    mapping_2d_scan_matching_proto_ceres_scan_matcher_options
+    mapping_2d_scan_matching_rotation_delta_cost_functor
+    mapping_2d_scan_matching_translation_delta_cost_functor
+    sensor_point_cloud
+    transform_transform
+)
+
+google_library(mapping_2d_scan_matching_correlative_scan_matcher
+  USES_EIGEN
+  SRCS
+    correlative_scan_matcher.cc
+  HDRS
+    correlative_scan_matcher.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_math
+    mapping_2d_map_limits
+    mapping_2d_xy_index
+    sensor_point_cloud
+)
+
+google_library(mapping_2d_scan_matching_fast_correlative_scan_matcher
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    fast_correlative_scan_matcher.cc
+  HDRS
+    fast_correlative_scan_matcher.h
+  DEPENDS
+    common_math
+    common_port
+    kalman_filter_pose_tracker
+    mapping_2d_probability_grid
+    mapping_2d_scan_matching_correlative_scan_matcher
+    mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
+    sensor_point_cloud
+    transform_transform
+)
+
+google_library(mapping_2d_scan_matching_fast_global_localizer
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    fast_global_localizer.cc
+  HDRS
+    fast_global_localizer.h
+  DEPENDS
+    kalman_filter_pose_tracker
+    mapping_2d_scan_matching_fast_correlative_scan_matcher
+    sensor_voxel_filter
+)
+
+google_library(mapping_2d_scan_matching_occupied_space_cost_functor
+  USES_CERES
+  USES_EIGEN
+  HDRS
+    occupied_space_cost_functor.h
+  DEPENDS
+    mapping_2d_probability_grid
+    sensor_point_cloud
+)
+
+google_library(mapping_2d_scan_matching_real_time_correlative_scan_matcher
+  USES_CERES
+  USES_EIGEN
+  SRCS
+    real_time_correlative_scan_matcher.cc
+  HDRS
+    real_time_correlative_scan_matcher.h
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_math
+    kalman_filter_pose_tracker
+    mapping_2d_probability_grid
+    mapping_2d_scan_matching_correlative_scan_matcher
+    mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options
+    sensor_point_cloud
+    transform_transform
+)
+
+google_library(mapping_2d_scan_matching_rotation_delta_cost_functor
+  USES_EIGEN
+  HDRS
+    rotation_delta_cost_functor.h
+  DEPENDS
+    transform_transform
+)
+
+google_library(mapping_2d_scan_matching_translation_delta_cost_functor
+  USES_EIGEN
+  HDRS
+    translation_delta_cost_functor.h
+)
+
+google_test(mapping_2d_scan_matching_ceres_scan_matcher_test
+  SRCS
+    ceres_scan_matcher_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary
+    common_lua_parameter_dictionary_test_helpers
+    common_make_unique
+    mapping_2d_probability_grid
+    mapping_2d_scan_matching_ceres_scan_matcher
+    sensor_point_cloud
+    transform_rigid_transform_test_helpers
+)
+
+google_test(mapping_2d_scan_matching_correlative_scan_matcher_test
+  SRCS
+    correlative_scan_matcher_test.cc
+  DEPENDS
+    mapping_2d_scan_matching_correlative_scan_matcher
+    sensor_point_cloud
+)
+
+google_test(mapping_2d_scan_matching_fast_correlative_scan_matcher_test
+  SRCS
+    fast_correlative_scan_matcher_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary_test_helpers
+    mapping_2d_laser_fan_inserter
+    mapping_2d_probability_grid
+    mapping_2d_scan_matching_fast_correlative_scan_matcher
+    transform_rigid_transform_test_helpers
+    transform_transform
+)
+
+google_test(mapping_2d_scan_matching_real_time_correlative_scan_matcher_test
+  USES_EIGEN
+  SRCS
+    real_time_correlative_scan_matcher_test.cc
+  DEPENDS
+    common_lua_parameter_dictionary_test_helpers
+    common_make_unique
+    kalman_filter_pose_tracker
+    mapping_2d_laser_fan_inserter
+    mapping_2d_probability_grid
+    mapping_2d_scan_matching_real_time_correlative_scan_matcher
+    sensor_point_cloud
+    transform_transform
+)