commit 166f2568bc278802b1e2116a468c961d22d9ca24 Author: Damon Kohler Date: Tue Aug 2 09:07:31 2016 +0200 Initial import of Cartographer codebase. diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 0000000..663aac4 --- /dev/null +++ b/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 diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..2827b7d --- /dev/null +++ b/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). diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..d9dd21c --- /dev/null +++ b/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 + git clone git@github.com:ros-drivers/velodyne.git + cd /src + ln -s /velodyne/velodyne* . diff --git a/cartographer/CMakeLists.txt b/cartographer/CMakeLists.txt new file mode 100644 index 0000000..0fcccdd --- /dev/null +++ b/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/ +) diff --git a/cartographer/cartographer-config.cmake.in b/cartographer/cartographer-config.cmake.in new file mode 100644 index 0000000..1aa5c39 --- /dev/null +++ b/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() diff --git a/cartographer/cartographer/CMakeLists.txt b/cartographer/cartographer/CMakeLists.txt new file mode 100644 index 0000000..9982b30 --- /dev/null +++ b/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") diff --git a/cartographer/cartographer/common/CMakeLists.txt b/cartographer/cartographer/common/CMakeLists.txt new file mode 100644 index 0000000..b0aea2c --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/common/blocking_queue.h b/cartographer/cartographer/common/blocking_queue.h new file mode 100644 index 0000000..ea53d56 --- /dev/null +++ b/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 +#include +#include + +#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 +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 + 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 deque_ GUARDED_BY(mutex_); +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_ diff --git a/cartographer/cartographer/common/blocking_queue_test.cc b/cartographer/cartographer/common/blocking_queue_test.cc new file mode 100644 index 0000000..92bdc0c --- /dev/null +++ b/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 +#include + +#include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +TEST(BlockingQueueTest, testPushPeekPop) { + BlockingQueue> blocking_queue; + blocking_queue.Push(common::make_unique(42)); + ASSERT_EQ(1, blocking_queue.Size()); + blocking_queue.Push(common::make_unique(24)); + ASSERT_EQ(2, blocking_queue.Size()); + EXPECT_EQ(42, *blocking_queue.Peek()); + 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()); + ASSERT_EQ(0, blocking_queue.Size()); +} + +TEST(BlockingQueueTest, testPushPopSharedPtr) { + BlockingQueue> blocking_queue; + blocking_queue.Push(std::make_shared(42)); + blocking_queue.Push(std::make_shared(24)); + EXPECT_EQ(42, *blocking_queue.Pop()); + EXPECT_EQ(24, *blocking_queue.Pop()); +} + +TEST(BlockingQueueTest, testPopWithTimeout) { + BlockingQueue> blocking_queue; + EXPECT_EQ(nullptr, + blocking_queue.PopWithTimeout(common::FromMilliseconds(150))); +} + +TEST(BlockingQueueTest, testPushWithTimeout) { + BlockingQueue> blocking_queue(1); + EXPECT_EQ(true, + blocking_queue.PushWithTimeout(common::make_unique(42), + common::FromMilliseconds(150))); + EXPECT_EQ(false, + blocking_queue.PushWithTimeout(common::make_unique(15), + common::FromMilliseconds(150))); + EXPECT_EQ(42, *blocking_queue.Pop()); + EXPECT_EQ(0, blocking_queue.Size()); +} + +TEST(BlockingQueueTest, testPushWithTimeoutInfinteQueue) { + BlockingQueue> blocking_queue; + EXPECT_EQ(true, + blocking_queue.PushWithTimeout(common::make_unique(42), + common::FromMilliseconds(150))); + EXPECT_EQ(true, + blocking_queue.PushWithTimeout(common::make_unique(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> 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(42)); + thread.join(); + ASSERT_EQ(0, blocking_queue.Size()); + EXPECT_EQ(42, pop); +} + +TEST(BlockingQueueTest, testBlockingPopWithTimeout) { + BlockingQueue> 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(42)); + thread.join(); + ASSERT_EQ(0, blocking_queue.Size()); + EXPECT_EQ(42, pop); +} + +} // namespace +} // namespace common +} // namespace cartographer diff --git a/cartographer/cartographer/common/ceres_solver_options.cc b/cartographer/cartographer/common/ceres_solver_options.cc new file mode 100644 index 0000000..25cd558 --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/common/ceres_solver_options.h b/cartographer/cartographer/common/ceres_solver_options.h new file mode 100644 index 0000000..cb5f35f --- /dev/null +++ b/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_ diff --git a/cartographer/cartographer/common/config.h.cmake b/cartographer/cartographer/common/config.h.cmake new file mode 100644 index 0000000..4658986 --- /dev/null +++ b/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_ diff --git a/cartographer/cartographer/common/configuration_file_resolver.cc b/cartographer/cartographer/common/configuration_file_resolver.cc new file mode 100644 index 0000000..6ceb6ec --- /dev/null +++ b/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 +#include +#include + +#include "cartographer/common/config.h" +#include "glog/logging.h" + +namespace cartographer { +namespace common { + +ConfigurationFileResolver::ConfigurationFileResolver( + const std::vector& 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(stream)), + std::istreambuf_iterator()); +} + +} // namespace common +} // namespace cartographer diff --git a/cartographer/cartographer/common/configuration_file_resolver.h b/cartographer/cartographer/common/configuration_file_resolver.h new file mode 100644 index 0000000..346a5d6 --- /dev/null +++ b/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 + +#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& configuration_files_directories); + + string GetFullPathOrDie(const string& basename) override; + string GetFileContentOrDie(const string& basename) override; + + private: + std::vector configuration_files_directories_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ diff --git a/cartographer/cartographer/common/fixed_ratio_sampler.cc b/cartographer/cartographer/common/fixed_ratio_sampler.cc new file mode 100644 index 0000000..7de8c77 --- /dev/null +++ b/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(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 diff --git a/cartographer/cartographer/common/fixed_ratio_sampler.h b/cartographer/cartographer/common/fixed_ratio_sampler.h new file mode 100644 index 0000000..f67d5b2 --- /dev/null +++ b/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 + +#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_ diff --git a/cartographer/cartographer/common/fixed_ratio_sampler_test.cc b/cartographer/cartographer/common/fixed_ratio_sampler_test.cc new file mode 100644 index 0000000..3bce575 --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/common/histogram.cc b/cartographer/cartographer/common/histogram.cc new file mode 100644 index 0000000..2dbb286 --- /dev/null +++ b/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 +#include +#include + +#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(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 diff --git a/cartographer/cartographer/common/histogram.h b/cartographer/cartographer/common/histogram.h new file mode 100644 index 0000000..ec49a4e --- /dev/null +++ b/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 +#include +#include + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace common { + +class BucketHistogram { + public: + void Hit(const string& bucket); + string ToString() const; + + private: + std::map buckets_; +}; + +class Histogram { + public: + void Add(float value); + string ToString(int buckets) const; + + private: + std::vector values_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_HISTOGRAM_H_ diff --git a/cartographer/cartographer/common/lua.h b/cartographer/cartographer/common/lua.h new file mode 100644 index 0000000..a40f4f0 --- /dev/null +++ b/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 + +#endif // CARTOGRAPHER_COMMON_LUA_H_ diff --git a/cartographer/cartographer/common/lua_parameter_dictionary.cc b/cartographer/cartographer/common/lua_parameter_dictionary.cc new file mode 100644 index 0000000..6fca85a --- /dev/null +++ b/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: ... + +#include "cartographer/common/lua_parameter_dictionary.h" + +#include +#include +#include +#include + +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 + lua_getfield(L, -1, + "format"); // S: ... string globals format + lua_pushstring(L, "%q"); // S: ... string globals format "%q" + lua_pushvalue(L, current_index); // S: ... string globals + // format "%q" string + + lua_call(L, 2, 1); // S: ... string globals quoted + lua_replace(L, current_index); // S: ... quoted globals + + 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(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 +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 +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& 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::NonReferenceCounted( + const string& code, std::unique_ptr file_resolver, + StateExtensionFunction state_extension_function) { + return std::unique_ptr(new LuaParameterDictionary( + code, ReferenceCount::NO, std::move(file_resolver), + state_extension_function)); +} + +std::unique_ptr LuaParameterDictionary::Partial( + const string& code, const string& key, + std::unique_ptr file_resolver, + StateExtensionFunction state_extension_function) { + auto parameter_dictionary = + std::unique_ptr(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 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 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 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 LuaParameterDictionary::GetKeys() const { + CheckTableIsAtTopOfStack(L_); + std::vector 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::GetDictionary( + const string& key) { + CheckHasKeyAndReference(key); + GetValueFromLuaTable(L_, key); + return PopDictionary(reference_count_); +} + +std::unique_ptr LuaParameterDictionary::PopDictionary( + ReferenceCount reference_count) const { + CheckTableIsAtTopOfStack(L_); + std::unique_ptr 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 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 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 LuaParameterDictionary::GetArrayValuesAsDoubles() { + std::vector values; + GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); }); + return values; +} + +std::vector> +LuaParameterDictionary::GetArrayValuesAsDictionaries() { + std::vector> values; + GetArrayValues(L_, [&values, this] { + values.push_back(PopDictionary(reference_count_)); + }); + return values; +} + +std::vector LuaParameterDictionary::GetArrayValuesAsStrings() { + std::vector 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 diff --git a/cartographer/cartographer/common/lua_parameter_dictionary.h b/cartographer/cartographer/common/lua_parameter_dictionary.h new file mode 100644 index 0000000..207b0be --- /dev/null +++ b/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 +#include +#include +#include + +#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; + +// 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 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 NonReferenceCounted( + const string& code, std::unique_ptr 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 Partial( + const string& code, const string& key, + std::unique_ptr file_resolver, + StateExtensionFunction state_extension_functon); + + ~LuaParameterDictionary(); + + // Returns all available keys. + std::vector 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 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 GetArrayValuesAsDoubles(); + std::vector GetArrayValuesAsStrings(); + std::vector> + GetArrayValuesAsDictionaries(); + + private: + enum class ReferenceCount { YES, NO }; + LuaParameterDictionary(const string& code, ReferenceCount reference_count, + std::unique_ptr file_resolver, + StateExtensionFunction state_extension_function); + + // For GetDictionary(). + LuaParameterDictionary(lua_State* L, ReferenceCount reference_count, + std::shared_ptr 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 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 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 reference_counts_; + + // List of all included files in order of inclusion. Used to prevent double + // inclusion. + std::vector included_files_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_ diff --git a/cartographer/cartographer/common/lua_parameter_dictionary_test.cc b/cartographer/cartographer/common/lua_parameter_dictionary_test.cc new file mode 100644 index 0000000..b7d686c --- /dev/null +++ b/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 +#include +#include + +#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 MakeNonReferenceCounted( + const string& code) { + return LuaParameterDictionary::NonReferenceCounted( + code, common::make_unique(), + nullptr /* state_extension_function */); +} + +std::unique_ptr MakePartial(const string& code, + const string& key) { + return LuaParameterDictionary::Partial( + code, key, common::make_unique(), + 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 sub_dict(dict->GetDictionary("blah")); + std::vector 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 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 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 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> 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 diff --git a/cartographer/cartographer/common/lua_parameter_dictionary_test_helpers.h b/cartographer/cartographer/common/lua_parameter_dictionary_test_helpers.h new file mode 100644 index 0000000..6d269a8 --- /dev/null +++ b/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 +#include + +#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 MakeDictionary(const string& code) { + return common::make_unique( + code, std::unique_ptr(new DummyFileResolver()), + nullptr /* state_extension_function */); +} + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ diff --git a/cartographer/cartographer/common/make_unique.h b/cartographer/cartographer/common/make_unique.h new file mode 100644 index 0000000..a31baa9 --- /dev/null +++ b/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 +#include +#include +#include + +namespace cartographer { +namespace common { + +// Implementation of c++14's std::make_unique, taken from +// https://isocpp.org/files/papers/N3656.txt +template +struct _Unique_if { + typedef std::unique_ptr _Single_object; +}; + +template +struct _Unique_if { + typedef std::unique_ptr _Unknown_bound; +}; + +template +struct _Unique_if { + typedef void _Known_bound; +}; + +template +typename _Unique_if::_Single_object make_unique(Args&&... args) { + return std::unique_ptr(new T(std::forward(args)...)); +} + +template +typename _Unique_if::_Unknown_bound make_unique(size_t n) { + typedef typename std::remove_extent::type U; + return std::unique_ptr(new U[n]()); +} + +template +typename _Unique_if::_Known_bound make_unique(Args&&...) = delete; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ diff --git a/cartographer/cartographer/common/math.h b/cartographer/cartographer/common/math.h new file mode 100644 index 0000000..03f38b7 --- /dev/null +++ b/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 +#include + +#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 +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 +constexpr T Power(T base, int exponent) { + return (exponent != 0) ? base * Power(base, exponent - 1) : T(1); +} + +// Calculates a^2. +template +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 +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 +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 +T atan2(const Eigen::Matrix& 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 +Eigen::Matrix ComputeSpdMatrixSqrtInverse( + const Eigen::Matrix& A, const double scale, + const double lower_eigenvalue_bound) { + Eigen::SelfAdjointEigenSolver> + covariance_eigen_solver(A / scale); + if (covariance_eigen_solver.info() != Eigen::Success) { + LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A; + return Eigen::Matrix::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_ diff --git a/cartographer/cartographer/common/math_test.cc b/cartographer/cartographer/common/math_test.cc new file mode 100644 index 0000000..cad7c1a --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/common/mutex.h b/cartographer/cartographer/common/mutex.h new file mode 100644 index 0000000..32e8eea --- /dev/null +++ b/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 +#include + +#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 + void Await(Predicate predicate) REQUIRES(this) { + mutex_->condition_.wait(lock_, predicate); + } + + template + bool AwaitWithTimeout(Predicate predicate, common::Duration timeout) + REQUIRES(this) { + return mutex_->condition_.wait_for(lock_, timeout, predicate); + } + + private: + Mutex* mutex_; + std::unique_lock lock_; + }; + + private: + std::condition_variable condition_; + std::mutex mutex_; +}; + +using MutexLocker = Mutex::Locker; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_MUTEX_H_ diff --git a/cartographer/cartographer/common/ordered_multi_queue.h b/cartographer/cartographer/common/ordered_multi_queue.h new file mode 100644 index 0000000..aeec4b0 --- /dev/null +++ b/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 +#include +#include + +#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 +class OrderedMultiQueue { + public: + using Callback = std::function)>; + + // 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 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(sort_key, std::move(value))); + Dispatch(); + } + + // Dispatches all remaining values in sorted order and removes the underlying + // queues. + void Flush() { + std::vector 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 value) + : sort_key(sort_key), value(std::move(value)) {} + SortKeyType sort_key; + std::unique_ptr value; + }; + + struct Queue { + common::BlockingQueue> 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(); + 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 queues_; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_ diff --git a/cartographer/cartographer/common/ordered_multi_queue_test.cc b/cartographer/cartographer/common/ordered_multi_queue_test.cc new file mode 100644 index 0000000..68c9820 --- /dev/null +++ b/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 + +#include "cartographer/common/make_unique.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace common { +namespace { + +TEST(OrderedMultiQueue, Ordering) { + std::vector values; + OrderedMultiQueue queue; + for (int i : {1, 2, 3}) { + queue.AddQueue(i, [&values](std::unique_ptr value) { + if (!values.empty()) { + EXPECT_GT(*value, values.back()); + } + values.push_back(*value); + }); + } + queue.Add(1, 4, common::make_unique(4)); + queue.Add(1, 5, common::make_unique(5)); + queue.Add(1, 6, common::make_unique(6)); + EXPECT_TRUE(values.empty()); + queue.Add(2, 1, common::make_unique(1)); + EXPECT_TRUE(values.empty()); + queue.Add(3, 2, common::make_unique(2)); + EXPECT_EQ(values.size(), 1); + queue.Add(2, 3, common::make_unique(3)); + EXPECT_EQ(values.size(), 2); + queue.Add(2, 7, common::make_unique(7)); + queue.Add(3, 8, common::make_unique(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 values; + OrderedMultiQueue queue; + for (int i : {1, 2, 3}) { + queue.AddQueue(i, [&values](std::unique_ptr value) { + if (!values.empty()) { + EXPECT_GT(*value, values.back()); + } + values.push_back(*value); + }); + } + queue.Add(1, 1, common::make_unique(1)); + queue.Add(1, 2, common::make_unique(2)); + queue.Add(1, 3, common::make_unique(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 diff --git a/cartographer/cartographer/common/port.h b/cartographer/cartographer/common/port.h new file mode 100644 index 0000000..c32bae4 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +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(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(compressed.data()), + compressed.size()); +} + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_PORT_H_ diff --git a/cartographer/cartographer/common/proto/CMakeLists.txt b/cartographer/cartographer/common/proto/CMakeLists.txt new file mode 100644 index 0000000..cb8dadd --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/common/proto/ceres_solver_options.proto b/cartographer/cartographer/common/proto/ceres_solver_options.proto new file mode 100644 index 0000000..c5f6bc5 --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/common/thread_pool.cc b/cartographer/cartographer/common/thread_pool.cc new file mode 100644 index 0000000..12e84d5 --- /dev/null +++ b/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 +#include +#include +#include + +#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 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 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 diff --git a/cartographer/cartographer/common/thread_pool.h b/cartographer/cartographer/common/thread_pool.h new file mode 100644 index 0000000..b93da78 --- /dev/null +++ b/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 +#include +#include +#include + +#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 work_item); + + private: + void DoWork(); + + Mutex mutex_; + bool running_ GUARDED_BY(mutex_) = true; + std::vector pool_ GUARDED_BY(mutex_); + std::deque> work_queue_ GUARDED_BY(mutex_); +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_THREAD_POOL_H_ diff --git a/cartographer/cartographer/common/time.cc b/cartographer/cartographer/common/time.cc new file mode 100644 index 0000000..760ebc0 --- /dev/null +++ b/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 + +namespace cartographer { +namespace common { + +Duration FromSeconds(const double seconds) { + return Duration(static_cast(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 diff --git a/cartographer/cartographer/common/time.h b/cartographer/cartographer/common/time.h new file mode 100644 index 0000000..08a32a3 --- /dev/null +++ b/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 +#include +#include + +#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; + using time_point = std::chrono::time_point; + 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_ diff --git a/cartographer/cartographer/kalman_filter/CMakeLists.txt b/cartographer/cartographer/kalman_filter/CMakeLists.txt new file mode 100644 index 0000000..0c3f085 --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/kalman_filter/gaussian_distribution.h b/cartographer/cartographer/kalman_filter/gaussian_distribution.h new file mode 100644 index 0000000..45f905a --- /dev/null +++ b/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 +class GaussianDistribution { + public: + GaussianDistribution(const Eigen::Matrix& mean, + const Eigen::Matrix& covariance) + : mean_(mean), covariance_(covariance) {} + + const Eigen::Matrix& GetMean() const { return mean_; } + + const Eigen::Matrix& GetCovariance() const { return covariance_; } + + private: + Eigen::Matrix mean_; + Eigen::Matrix covariance_; +}; + +template +GaussianDistribution operator+(const GaussianDistribution& lhs, + const GaussianDistribution& rhs) { + return GaussianDistribution(lhs.GetMean() + rhs.GetMean(), + lhs.GetCovariance() + rhs.GetCovariance()); +} + +template +GaussianDistribution operator*(const Eigen::Matrix& lhs, + const GaussianDistribution& rhs) { + return GaussianDistribution( + lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose()); +} + +} // namespace kalman_filter +} // namespace cartographer + +#endif // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ diff --git a/cartographer/cartographer/kalman_filter/gaussian_distribution_test.cc b/cartographer/cartographer/kalman_filter/gaussian_distribution_test.cc new file mode 100644 index 0000000..c5e5e1d --- /dev/null +++ b/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 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 diff --git a/cartographer/cartographer/kalman_filter/odometry_state_tracker.cc b/cartographer/cartographer/kalman_filter/odometry_state_tracker.cc new file mode 100644 index 0000000..2ffc557 --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/kalman_filter/odometry_state_tracker.h b/cartographer/cartographer/kalman_filter/odometry_state_tracker.h new file mode 100644 index 0000000..f02ca5a --- /dev/null +++ b/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 + +#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; + + 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_ diff --git a/cartographer/cartographer/kalman_filter/pose_tracker.cc b/cartographer/cartographer/kalman_filter/pose_tracker.cc new file mode 100644 index 0000000..36ff048 --- /dev/null +++ b/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 +#include +#include + +#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::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 distribution( + Eigen::Matrix::Zero(), pose_and_covariance.covariance); + Eigen::Matrix 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 delta( + Eigen::Matrix::Zero(), covariance); + + kalman_filter_.Observe<6>( + [this, &pose](const State& state) -> Eigen::Matrix { + 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 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 diff --git a/cartographer/cartographer/kalman_filter/pose_tracker.h b/cartographer/cartographer/kalman_filter/pose_tracker.h new file mode 100644 index 0000000..19871e4 --- /dev/null +++ b/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 +#include + +#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 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; + using State = KalmanFilter::StateType; + using StateCovariance = Eigen::Matrix; + using Distribution = GaussianDistribution; + + // 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_ diff --git a/cartographer/cartographer/kalman_filter/pose_tracker_test.cc b/cartographer/cartographer/kalman_filter/pose_tracker_test.cc new file mode 100644 index 0000000..c43145a --- /dev/null +++ b/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 + +#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( + options, PoseTracker::ModelFunction::k3D, common::FromUniversal(1000)); + } + + std::unique_ptr pose_tracker_; +}; + +TEST(CovarianceTest, EmbedAndProjectCovariance) { + std::mt19937 prng(42); + std::uniform_real_distribution 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 poses(3); + std::vector 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::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::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 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 diff --git a/cartographer/cartographer/kalman_filter/proto/CMakeLists.txt b/cartographer/cartographer/kalman_filter/proto/CMakeLists.txt new file mode 100644 index 0000000..70f7f68 --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/kalman_filter/proto/pose_tracker_options.proto b/cartographer/cartographer/kalman_filter/proto/pose_tracker_options.proto new file mode 100644 index 0000000..d34620c --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/kalman_filter/unscented_kalman_filter.h b/cartographer/cartographer/kalman_filter/unscented_kalman_filter.h new file mode 100644 index 0000000..d62c1b1 --- /dev/null +++ b/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 +#include +#include +#include + +#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 +constexpr FloatType sqr(FloatType a) { + return a * a; +} + +template +Eigen::Matrix OuterProduct( + const Eigen::Matrix& v) { + return v * v.transpose(); +} + +// Checks if 'A' is a symmetric matrix. +template +void CheckSymmetric(const Eigen::Matrix& 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 +Eigen::Matrix MatrixSqrt( + const Eigen::Matrix& A) { + CheckSymmetric(A); + + Eigen::SelfAdjointEigenSolver> + 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::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 +class UnscentedKalmanFilter { + public: + using StateType = Eigen::Matrix; + using StateCovarianceType = Eigen::Matrix; + + explicit UnscentedKalmanFilter( + const GaussianDistribution& initial_belief, + std::function + add_delta = [](const StateType& state, + const StateType& delta) { return state + delta; }, + std::function + 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 g, + const GaussianDistribution& 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 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(compute_delta_(new_mu, Y[0])); + for (int i = 0; i < N; ++i) { + new_sigma += kCovWeightI * OuterProduct( + compute_delta_(new_mu, Y[2 * i + 1])); + new_sigma += kCovWeightI * OuterProduct( + compute_delta_(new_mu, Y[2 * i + 2])); + } + CheckSymmetric(new_sigma); + + belief_ = GaussianDistribution(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 + void Observe( + std::function(const StateType&)> h, + const GaussianDistribution& 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 W; + W.reserve(2 * N + 1); + W.emplace_back(StateType::Zero()); + + std::vector> Z; + Z.reserve(2 * N + 1); + Z.emplace_back(h(mu)); + + Eigen::Matrix 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 S = + kCovWeight0 * OuterProduct(Z[0] - z_hat); + for (int i = 0; i < N; ++i) { + S += kCovWeightI * OuterProduct(Z[2 * i + 1] - z_hat); + S += kCovWeightI * OuterProduct(Z[2 * i + 2] - z_hat); + } + CheckSymmetric(S); + S += delta.GetCovariance(); + + Eigen::Matrix 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 kalman_gain = + sigma_bar_xz * S.inverse(); + const StateCovarianceType new_sigma = + belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose(); + CheckSymmetric(new_sigma); + + belief_ = GaussianDistribution( + add_delta_(mu, kalman_gain * -z_hat), new_sigma); + } + + const GaussianDistribution& GetBelief() const { + return belief_; + } + + private: + StateType ComputeWeightedError(const StateType& mean_estimate, + const std::vector& 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& 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 belief_; + const std::function + add_delta_; + const std::function + compute_delta_; +}; + +template +constexpr FloatType UnscentedKalmanFilter::kAlpha; +template +constexpr FloatType UnscentedKalmanFilter::kKappa; +template +constexpr FloatType UnscentedKalmanFilter::kBeta; +template +constexpr FloatType UnscentedKalmanFilter::kLambda; +template +constexpr FloatType UnscentedKalmanFilter::kMeanWeight0; +template +constexpr FloatType UnscentedKalmanFilter::kCovWeight0; +template +constexpr FloatType UnscentedKalmanFilter::kMeanWeightI; +template +constexpr FloatType UnscentedKalmanFilter::kCovWeightI; + +} // namespace kalman_filter +} // namespace cartographer + +#endif // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_ diff --git a/cartographer/cartographer/kalman_filter/unscented_kalman_filter_test.cc b/cartographer/cartographer/kalman_filter/unscented_kalman_filter_test.cc new file mode 100644 index 0000000..08d3658 --- /dev/null +++ b/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 Scalar(double value) { + return value * Eigen::Matrix::Identity(); +} + +// A simple model that copies the first to the second state variable. +Eigen::Matrix g(const Eigen::Matrix& state) { + Eigen::Matrix new_state; + new_state << state[0], state[0]; + return new_state; +} + +// A simple observation of the first state variable. +Eigen::Matrix h(const Eigen::Matrix& state) { + return Scalar(state[0]) - Scalar(5.); +} + +TEST(KalmanFilterTest, testConstructor) { + UnscentedKalmanFilter filter(GaussianDistribution( + Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity())); + EXPECT_NEAR(42., filter.GetBelief().GetMean()[1], 1e-9); +} + +TEST(KalmanFilterTest, testPredict) { + UnscentedKalmanFilter filter(GaussianDistribution( + Eigen::Vector2d(42., 0.), 10. * Eigen::Matrix2d::Identity())); + filter.Predict( + g, GaussianDistribution(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 filter(GaussianDistribution( + Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity())); + for (int i = 0; i < 500; ++i) { + filter.Predict( + g, GaussianDistribution(Eigen::Vector2d(0., 0.), + Eigen::Matrix2d::Identity() * 1e-9)); + filter.Observe<1>( + h, GaussianDistribution(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 diff --git a/cartographer/cartographer/mapping/CMakeLists.txt b/cartographer/cartographer/mapping/CMakeLists.txt new file mode 100644 index 0000000..f2bdda6 --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/mapping/global_trajectory_builder_interface.cc b/cartographer/cartographer/mapping/global_trajectory_builder_interface.cc new file mode 100644 index 0000000..9c3fccc --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/cartographer/mapping/global_trajectory_builder_interface.h new file mode 100644 index 0000000..769d8ac --- /dev/null +++ b/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 +#include +#include + +#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_ diff --git a/cartographer/cartographer/mapping/probability_values.cc b/cartographer/cartographer/mapping/probability_values.cc new file mode 100644 index 0000000..32e45a4 --- /dev/null +++ b/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* PrecomputeValueToProbability() { + std::vector* result = new std::vector; + // 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* const kValueToProbability = + PrecomputeValueToProbability(); + +std::vector ComputeLookupTableToApplyOdds(const float odds) { + std::vector 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 diff --git a/cartographer/cartographer/mapping/probability_values.h b/cartographer/cartographer/mapping/probability_values.h new file mode 100644 index 0000000..c1993ba --- /dev/null +++ b/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 +#include + +#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* 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 ComputeLookupTableToApplyOdds(float odds); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ diff --git a/cartographer/cartographer/mapping/probability_values_test.cc b/cartographer/cartographer/mapping/probability_values_test.cc new file mode 100644 index 0000000..10aa2cf --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/mapping/proto/CMakeLists.txt b/cartographer/cartographer/mapping/proto/CMakeLists.txt new file mode 100644 index 0000000..71e79d1 --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/mapping/proto/scan_matching_progress.proto b/cartographer/cartographer/mapping/proto/scan_matching_progress.proto new file mode 100644 index 0000000..301ac18 --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping/proto/sparse_pose_graph_options.proto b/cartographer/cartographer/mapping/proto/sparse_pose_graph_options.proto new file mode 100644 index 0000000..1289eba --- /dev/null +++ b/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; +}; diff --git a/cartographer/cartographer/mapping/proto/submaps.proto b/cartographer/cartographer/mapping/proto/submaps.proto new file mode 100644 index 0000000..f98a4d2 --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping/proto/trajectory_connectivity.proto b/cartographer/cartographer/mapping/proto/trajectory_connectivity.proto new file mode 100644 index 0000000..e435a3f --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping/sensor_collator.h b/cartographer/cartographer/mapping/sensor_collator.h new file mode 100644 index 0000000..acd6fde --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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 +class SensorCollator { + public: + using Callback = std::function)>; + + 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& 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) { + 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 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{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::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 sensor_data; + }; + + // Queue keys are a pair of trajectory ID and sensor identifier. + common::OrderedMultiQueue queue_; + + // Map of trajectory ID to all associated QueueKeys. + std::unordered_map> queue_keys_; + sensor::SensorPacketPeriodHistogramBuilder + sensor_packet_period_histogram_builder_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_ diff --git a/cartographer/cartographer/mapping/sensor_collator_test.cc b/cartographer/cartographer/mapping/sensor_collator_test.cc new file mode 100644 index 0000000..99c2451 --- /dev/null +++ b/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 + +#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 frame_ids = { + "horizontal_laser", "vertical_laser", "imu", "something"}; + std::vector> received; + SensorCollator sensor_collator; + sensor_collator.AddTrajectory( + 0, frame_ids, + [&received](const int64 timestamp, std::unique_ptr packet) { + received.push_back(std::make_pair(timestamp, *packet)); + }); + + sensor_collator.AddSensorData(0, 100, first.frame_id, + common::make_unique(first)); + sensor_collator.AddSensorData(0, 600, sixth.frame_id, + common::make_unique(sixth)); + sensor_collator.AddSensorData(0, 400, fourth.frame_id, + common::make_unique(fourth)); + sensor_collator.AddSensorData(0, 200, second.frame_id, + common::make_unique(second)); + sensor_collator.AddSensorData(0, 500, fifth.frame_id, + common::make_unique(fifth)); + sensor_collator.AddSensorData(0, 300, third.frame_id, + common::make_unique(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 diff --git a/cartographer/cartographer/mapping/sparse_pose_graph.cc b/cartographer/cartographer/mapping/sparse_pose_graph.cc new file mode 100644 index 0000000..1412ced --- /dev/null +++ b/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 + +#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> SplitTrajectoryNodes( + const std::vector& trajectory_nodes) { + std::vector> trajectories; + std::unordered_map 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 diff --git a/cartographer/cartographer/mapping/sparse_pose_graph.h b/cartographer/cartographer/mapping/sparse_pose_graph.h new file mode 100644 index 0000000..c429ee9 --- /dev/null +++ b/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 + +#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> SplitTrajectoryNodes( + const std::vector& 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 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 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> + GetConnectedTrajectories() = 0; + + // Returns the current optimized transforms for the given 'submaps'. + virtual std::vector 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 GetTrajectoryNodes() = 0; + + // Returns the collection of 2D constraints. + virtual std::vector constraints_2d() = 0; + + // Returns the collection of 3D constraints. + virtual std::vector constraints_3d() = 0; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/CMakeLists.txt b/cartographer/cartographer/mapping/sparse_pose_graph/CMakeLists.txt new file mode 100644 index 0000000..0b9e21d --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/cartographer/mapping/sparse_pose_graph/constraint_builder.cc new file mode 100644 index 0000000..326a990 --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/constraint_builder.h b/cartographer/cartographer/mapping/sparse_pose_graph/constraint_builder.h new file mode 100644 index 0000000..63820e4 --- /dev/null +++ b/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_ diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc b/cartographer/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc new file mode 100644 index 0000000..4220b13 --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/optimization_problem_options.h b/cartographer/cartographer/mapping/sparse_pose_graph/optimization_problem_options.h new file mode 100644 index 0000000..f7b138c --- /dev/null +++ b/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_ diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/proto/CMakeLists.txt b/cartographer/cartographer/mapping/sparse_pose_graph/proto/CMakeLists.txt new file mode 100644 index 0000000..8adab1c --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto b/cartographer/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto new file mode 100644 index 0000000..fddd5d2 --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto b/cartographer/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto new file mode 100644 index 0000000..20d1d79 --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping/sparse_pose_graph_test.cc b/cartographer/cartographer/mapping/sparse_pose_graph_test.cc new file mode 100644 index 0000000..681afe3 --- /dev/null +++ b/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 + +#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&, + const transform::Rigid3d&, + proto::SubmapQuery::Response*) override { + LOG(FATAL) << "Not implemented."; + } +}; + +TEST(SparsePoseGraphTest, SplitTrajectoryNodes) { + std::vector trajectory_nodes; + const std::vector submaps(5); + std::deque 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> 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 diff --git a/cartographer/cartographer/mapping/submaps.cc b/cartographer/cartographer/mapping/submaps.cc new file mode 100644 index 0000000..eb65250 --- /dev/null +++ b/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 + +#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 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(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 diff --git a/cartographer/cartographer/mapping/submaps.h b/cartographer/cartographer/mapping/submaps.h new file mode 100644 index 0000000..ae3f698 --- /dev/null +++ b/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 +#include + +#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()); + } + + // 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 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& 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_ diff --git a/cartographer/cartographer/mapping/submaps_test.cc b/cartographer/cartographer/mapping/submaps_test.cc new file mode 100644 index 0000000..dd2e1eb --- /dev/null +++ b/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 + +#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 diff --git a/cartographer/cartographer/mapping/trajectory_connectivity.cc b/cartographer/cartographer/mapping/trajectory_connectivity.cc new file mode 100644 index 0000000..24a0144 --- /dev/null +++ b/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 +#include + +#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()); + ++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> +TrajectoryConnectivity::ConnectedComponents() { + // Map from cluster exemplar -> growing cluster. + std::unordered_map> map; + common::MutexLocker locker(&lock_); + for (const auto& trajectory_entry : forest_) { + map[FindSet(trajectory_entry.first)].push_back(trajectory_entry.first); + } + + std::vector> 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())); + return it != connection_map_.end() ? it->second : 0; +} + +proto::TrajectoryConnectivity ToProto( + std::vector> connected_components, + std::unordered_map trajectory_indices) { + proto::TrajectoryConnectivity proto; + std::vector> 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 diff --git a/cartographer/cartographer/mapping/trajectory_connectivity.h b/cartographer/cartographer/mapping/trajectory_connectivity.h new file mode 100644 index 0000000..dbc466a --- /dev/null +++ b/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 +#include + +#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> 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 forest_ GUARDED_BY(lock_); + // Tracks the number of direct connections between a pair of trajectories. + std::map, int> connection_map_ + GUARDED_BY(lock_); +}; + +// Returns a proto encoding connected components according to +// 'trajectory_indices'. +proto::TrajectoryConnectivity ToProto( + std::vector> connected_components, + std::unordered_map 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_ diff --git a/cartographer/cartographer/mapping/trajectory_connectivity_test.cc b/cartographer/cartographer/mapping/trajectory_connectivity_test.cc new file mode 100644 index 0000000..7b2c780 --- /dev/null +++ b/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 +#include +#include + +#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> 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* zero_cluster = nullptr; + const std::vector* 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 diff --git a/cartographer/cartographer/mapping/trajectory_node.h b/cartographer/cartographer/mapping/trajectory_node.h new file mode 100644 index 0000000..e09ff22 --- /dev/null +++ b/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 +#include + +#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 constant_data; + std::vector trajectory_nodes; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ diff --git a/cartographer/cartographer/mapping_2d/CMakeLists.txt b/cartographer/cartographer/mapping_2d/CMakeLists.txt new file mode 100644 index 0000000..e12a53e --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/cartographer/mapping_2d/global_trajectory_builder.cc new file mode 100644 index 0000000..a413335 --- /dev/null +++ b/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 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 diff --git a/cartographer/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/cartographer/mapping_2d/global_trajectory_builder.h new file mode 100644 index 0000000..ba5022c --- /dev/null +++ b/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_ diff --git a/cartographer/cartographer/mapping_2d/laser_fan_inserter.cc b/cartographer/cartographer/mapping_2d/laser_fan_inserter.cc new file mode 100644 index 0000000..053343a --- /dev/null +++ b/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 + +#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 diff --git a/cartographer/cartographer/mapping_2d/laser_fan_inserter.h b/cartographer/cartographer/mapping_2d/laser_fan_inserter.h new file mode 100644 index 0000000..daae856 --- /dev/null +++ b/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 +#include + +#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& hit_table() const { return hit_table_; } + const std::vector& miss_table() const { return miss_table_; } + + private: + const proto::LaserFanInserterOptions options_; + const std::vector hit_table_; + const std::vector miss_table_; +}; + +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ diff --git a/cartographer/cartographer/mapping_2d/laser_fan_inserter_test.cc b/cartographer/cartographer/mapping_2d/laser_fan_inserter_test.cc new file mode 100644 index 0000000..f7be710 --- /dev/null +++ b/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 + +#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(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 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 diff --git a/cartographer/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/cartographer/mapping_2d/local_trajectory_builder.cc new file mode 100644 index 0000000..e6c6e3a --- /dev/null +++ b/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 + +#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::infinity(), + -std::numeric_limits::infinity(), + options_.horizontal_laser_min_z()), + Eigen::Vector3f(std::numeric_limits::infinity(), + std::numeric_limits::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::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(), 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())}; + + 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 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())); + + return common::make_unique(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( + options_.pose_tracker_options(), + kalman_filter::PoseTracker::ModelFunction::k2D, time); + } +} + +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/cartographer/mapping_2d/local_trajectory_builder.h new file mode 100644 index 0000000..d7786b5 --- /dev/null +++ b/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 + +#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 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 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 pose_tracker_; +}; + +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/cartographer/mapping_2d/map_limits.h b/cartographer/cartographer/mapping_2d/map_limits.h new file mode 100644 index 0000000..04bdc7f --- /dev/null +++ b/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 +#include + +#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& 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()); + } + + static Eigen::AlignedBox2f ComputeMapBoundingBox( + const std::vector& 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()); + 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()); + 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_ diff --git a/cartographer/cartographer/mapping_2d/map_limits_test.cc b/cartographer/cartographer/mapping_2d/map_limits_test.cc new file mode 100644 index 0000000..29d7e11 --- /dev/null +++ b/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 diff --git a/cartographer/cartographer/mapping_2d/probability_grid.h b/cartographer/cartographer/mapping_2d/probability_grid.h new file mode 100644 index 0000000..7bb9d03 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#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& 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 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 cells_; // Highest bit is update marker. + std::vector 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_ diff --git a/cartographer/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/cartographer/mapping_2d/probability_grid_test.cc new file mode 100644 index 0000000..b5abb5e --- /dev/null +++ b/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 + +#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 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 diff --git a/cartographer/cartographer/mapping_2d/proto/CMakeLists.txt b/cartographer/cartographer/mapping_2d/proto/CMakeLists.txt new file mode 100644 index 0000000..3a6342c --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/mapping_2d/proto/laser_fan_inserter_options.proto b/cartographer/cartographer/mapping_2d/proto/laser_fan_inserter_options.proto new file mode 100644 index 0000000..673df2d --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto new file mode 100644 index 0000000..6bac10d --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping_2d/proto/submaps_options.proto b/cartographer/cartographer/mapping_2d/proto/submaps_options.proto new file mode 100644 index 0000000..894ed8a --- /dev/null +++ b/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; +} diff --git a/cartographer/cartographer/mapping_2d/ray_casting.cc b/cartographer/cartographer/mapping_2d/ray_casting.cc new file mode 100644 index 0000000..10dab95 --- /dev/null +++ b/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& 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& hit_visitor, + const std::function& 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 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 diff --git a/cartographer/cartographer/mapping_2d/ray_casting.h b/cartographer/cartographer/mapping_2d/ray_casting.h new file mode 100644 index 0000000..7e12121 --- /dev/null +++ b/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 + +#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& hit_visitor, + const std::function& miss_visitor); + +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/CMakeLists.txt b/cartographer/cartographer/mapping_2d/scan_matching/CMakeLists.txt new file mode 100644 index 0000000..deadbc6 --- /dev/null +++ b/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 +) diff --git a/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc new file mode 100644 index 0000000..23e6a3f --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -0,0 +1,123 @@ +/* + * 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/scan_matching/ceres_scan_matcher.h" + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h" +#include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h" +#include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::CeresScanMatcherOptions options; + options.set_occupied_space_cost_functor_weight( + parameter_dictionary->GetDouble("occupied_space_cost_functor_weight")); + options.set_previous_pose_translation_delta_cost_functor_weight( + parameter_dictionary->GetDouble( + "previous_pose_translation_delta_cost_functor_weight")); + options.set_initial_pose_estimate_rotation_delta_cost_functor_weight( + parameter_dictionary->GetDouble( + "initial_pose_estimate_rotation_delta_cost_functor_weight")); + options.set_covariance_scale( + parameter_dictionary->GetDouble("covariance_scale")); + *options.mutable_ceres_solver_options() = + common::CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("ceres_solver_options").get()); + return options; +} + +CeresScanMatcher::CeresScanMatcher( + const proto::CeresScanMatcherOptions& options) + : options_(options), + ceres_solver_options_( + common::CreateCeresSolverOptions(options.ceres_solver_options())) { + ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; +} + +CeresScanMatcher::~CeresScanMatcher() {} + +void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, + const ProbabilityGrid& probability_grid, + transform::Rigid2d* const pose_estimate, + kalman_filter::Pose2DCovariance* const covariance, + ceres::Solver::Summary* const summary) const { + double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), + initial_pose_estimate.translation().y(), + initial_pose_estimate.rotation().angle()}; + ceres::Problem problem; + CHECK_GT(options_.occupied_space_cost_functor_weight(), 0.); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new OccupiedSpaceCostFunctor( + options_.occupied_space_cost_functor_weight() / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, probability_grid), + point_cloud.size()), + nullptr, ceres_pose_estimate); + CHECK_GT(options_.previous_pose_translation_delta_cost_functor_weight(), 0.); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new TranslationDeltaCostFunctor( + options_.previous_pose_translation_delta_cost_functor_weight(), + previous_pose)), + nullptr, ceres_pose_estimate); + CHECK_GT(options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), + 0.); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction(new RotationDeltaCostFunctor( + options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), + ceres_pose_estimate[2])), + nullptr, ceres_pose_estimate); + + ceres::Solve(ceres_solver_options_, &problem, summary); + + *pose_estimate = transform::Rigid2d( + {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]); + + ceres::Covariance::Options options; + ceres::Covariance covariance_computer(options); + std::vector> covariance_blocks; + covariance_blocks.emplace_back(ceres_pose_estimate, ceres_pose_estimate); + CHECK(covariance_computer.Compute(covariance_blocks, &problem)); + double ceres_covariance[3 * 3]; + covariance_computer.GetCovarianceBlock(ceres_pose_estimate, + ceres_pose_estimate, ceres_covariance); + *covariance = Eigen::Map(ceres_covariance); + *covariance *= options_.covariance_scale(); +} + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h b/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h new file mode 100644 index 0000000..97e403f --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h @@ -0,0 +1,67 @@ +/* + * 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_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( + common::LuaParameterDictionary* parameter_dictionary); + +// Align scans with an existing map using Ceres. +class CeresScanMatcher { + public: + explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options); + virtual ~CeresScanMatcher(); + + CeresScanMatcher(const CeresScanMatcher&) = delete; + CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; + + // Aligns 'point_cloud' within the 'probability_grid' given an + // 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and + // the solver 'summary'. + void Match(const transform::Rigid2d& previous_pose, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, + const ProbabilityGrid& probability_grid, + transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance, + ceres::Solver::Summary* summary) const; + + private: + const proto::CeresScanMatcherOptions options_; + ceres::Solver::Options ceres_solver_options_; +}; + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc new file mode 100644 index 0000000..a333127 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc @@ -0,0 +1,101 @@ +/* + * 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/scan_matching/ceres_scan_matcher.h" + +#include + +#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 "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { +namespace { + +class CeresScanMatcherTest : public ::testing::Test { + protected: + CeresScanMatcherTest() + : probability_grid_( + MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-10., -10.), + Eigen::Vector2d(10., 10.)))) { + probability_grid_.SetProbability( + probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5), + mapping::kMaxProbability); + + point_cloud_.emplace_back(-3., 2.); + + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + occupied_space_cost_functor_weight = 1., + previous_pose_translation_delta_cost_functor_weight = 0.1, + initial_pose_estimate_rotation_delta_cost_functor_weight = 1.5, + covariance_scale = 10., + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 50, + num_threads = 1, + }, + })text"); + const proto::CeresScanMatcherOptions options = + CreateCeresScanMatcherOptions(parameter_dictionary.get()); + ceres_scan_matcher_ = common::make_unique(options); + } + + void TestFromInitialPose(const transform::Rigid2d& initial_pose) { + transform::Rigid2d pose; + kalman_filter::Pose2DCovariance covariance; + const transform::Rigid2d expected_pose = + transform::Rigid2d::Translation({-0.5, 0.5}); + ceres::Solver::Summary summary; + ceres_scan_matcher_->Match(initial_pose, initial_pose, point_cloud_, + probability_grid_, &pose, &covariance, &summary); + EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); + EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2)) + << "Actual: " << transform::ToProto(pose).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + } + + ProbabilityGrid probability_grid_; + sensor::PointCloud2D point_cloud_; + std::unique_ptr ceres_scan_matcher_; +}; + +TEST_F(CeresScanMatcherTest, testPerfectEstimate) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.5, 0.5})); +} + +TEST_F(CeresScanMatcherTest, testOptimizeAlongX) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.5})); +} + +TEST_F(CeresScanMatcherTest, testOptimizeAlongY) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.45, 0.3})); +} + +TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) { + TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.3})); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc b/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc new file mode 100644 index 0000000..809aced --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc @@ -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. + */ + +#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" + +#include + +#include "cartographer/common/math.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +SearchParameters::SearchParameters(const double linear_search_window, + const double angular_search_window, + const sensor::PointCloud2D& point_cloud, + const double resolution) + : resolution(resolution) { + // We set this value to something on the order of resolution to make sure that + // the std::acos() below is defined. + float max_scan_range = 3.f * resolution; + for (const Eigen::Vector2f& point : point_cloud) { + const float range = point.norm(); + max_scan_range = std::max(range, max_scan_range); + } + const double kSafetyMargin = 1. - 1e-3; + angular_perturbation_step_size = + kSafetyMargin * + std::acos(1. - + common::Pow2(resolution) / (2. * common::Pow2(max_scan_range))); + num_angular_perturbations = + std::ceil(angular_search_window / angular_perturbation_step_size); + num_scans = 2 * num_angular_perturbations + 1; + + const int num_linear_perturbations = + std::ceil(linear_search_window / resolution); + linear_bounds.reserve(num_scans); + for (int i = 0; i != num_scans; ++i) { + linear_bounds.push_back( + LinearBounds{-num_linear_perturbations, num_linear_perturbations, + -num_linear_perturbations, num_linear_perturbations}); + } +} + +SearchParameters::SearchParameters(const int num_linear_perturbations, + const int num_angular_perturbations, + const double angular_perturbation_step_size, + const double resolution) + : num_angular_perturbations(num_angular_perturbations), + angular_perturbation_step_size(angular_perturbation_step_size), + resolution(resolution), + num_scans(2 * num_angular_perturbations + 1) { + linear_bounds.reserve(num_scans); + for (int i = 0; i != num_scans; ++i) { + linear_bounds.push_back( + LinearBounds{-num_linear_perturbations, num_linear_perturbations, + -num_linear_perturbations, num_linear_perturbations}); + } +} + +void SearchParameters::ShrinkToFit(const std::vector& scans, + const CellLimits& cell_limits) { + CHECK_EQ(scans.size(), num_scans); + CHECK_EQ(linear_bounds.size(), num_scans); + for (int i = 0; i != num_scans; ++i) { + Eigen::Array2i min_bound = Eigen::Array2i::Zero(); + Eigen::Array2i max_bound = Eigen::Array2i::Zero(); + for (const Eigen::Array2i& xy_index : scans[i]) { + min_bound = min_bound.min(-xy_index); + max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1, + cell_limits.num_y_cells - 1) - + xy_index); + } + linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x()); + linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x()); + linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y()); + linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y()); + } +} + +std::vector GenerateRotatedScans( + const sensor::PointCloud2D& point_cloud, + const SearchParameters& search_parameters) { + std::vector rotated_scans; + rotated_scans.reserve(search_parameters.num_scans); + + double delta_theta = -search_parameters.num_angular_perturbations * + search_parameters.angular_perturbation_step_size; + for (int scan_index = 0; scan_index < search_parameters.num_scans; + ++scan_index, + delta_theta += search_parameters.angular_perturbation_step_size) { + rotated_scans.push_back(sensor::TransformPointCloud2D( + point_cloud, transform::Rigid2f::Rotation(delta_theta))); + } + return rotated_scans; +} + +std::vector DiscretizeScans( + const MapLimits& map_limits, const std::vector& scans, + const Eigen::Translation2f& initial_translation) { + std::vector discrete_scans; + discrete_scans.reserve(scans.size()); + for (const sensor::PointCloud2D& scan : scans) { + discrete_scans.emplace_back(); + discrete_scans.back().reserve(scan.size()); + for (const Eigen::Vector2f& point : scan) { + const Eigen::Vector2f translated_point = initial_translation * point; + discrete_scans.back().push_back( + map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(), + translated_point.y())); + } + } + return discrete_scans; +} + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h b/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h new file mode 100644 index 0000000..a07bcaa --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h @@ -0,0 +1,109 @@ +/* + * 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_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping_2d/map_limits.h" +#include "cartographer/mapping_2d/xy_index.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +typedef std::vector DiscreteScan; + +// Describes the search space. +struct SearchParameters { + // Linear search window in pixel offsets; bounds are inclusive. + struct LinearBounds { + int min_x; + int max_x; + int min_y; + int max_y; + }; + + SearchParameters(double linear_search_window, double angular_search_window, + const sensor::PointCloud2D& point_cloud, double resolution); + + // For testing. + SearchParameters(int num_linear_perturbations, int num_angular_perturbations, + double angular_perturbation_step_size, double resolution); + + // Tightens the search window as much as possible. + void ShrinkToFit(const std::vector& scans, + const CellLimits& cell_limits); + + int num_angular_perturbations; + double angular_perturbation_step_size; + double resolution; + int num_scans; + std::vector linear_bounds; // Per rotated scans. +}; + +// Generates a collection of rotated scans. +std::vector GenerateRotatedScans( + const sensor::PointCloud2D& point_cloud, + const SearchParameters& search_parameters); + +// Translates and discretizes the rotated scans into a vector of integer +// indices. +std::vector DiscretizeScans( + const MapLimits& map_limits, const std::vector& scans, + const Eigen::Translation2f& initial_translation); + +// A possible solution. +struct Candidate { + Candidate(const int init_scan_index, const int init_x_index_offset, + const int init_y_index_offset, + const SearchParameters& search_parameters) + : scan_index(init_scan_index), + x_index_offset(init_x_index_offset), + y_index_offset(init_y_index_offset), + x(-y_index_offset * search_parameters.resolution), + y(-x_index_offset * search_parameters.resolution), + orientation((scan_index - search_parameters.num_angular_perturbations) * + search_parameters.angular_perturbation_step_size) {} + + // Index into the rotated scans vector. + int scan_index = 0; + + // Linear offset from the initial pose. + int x_index_offset = 0; + int y_index_offset = 0; + + // Pose of this Candidate relative to the initial pose. + double x = 0.; + double y = 0.; + double orientation = 0.; + + // Score, higher is better. + float score = 0.f; + + bool operator<(const Candidate& other) const { return score < other.score; } + bool operator>(const Candidate& other) const { return score > other.score; } +}; + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc b/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc new file mode 100644 index 0000000..231ee7a --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc @@ -0,0 +1,103 @@ +/* + * 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/scan_matching/correlative_scan_matcher.h" + +#include "cartographer/sensor/point_cloud.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { +namespace { + +TEST(SearchParameters, Construction) { + const SearchParameters search_parameters(4, 5, 0.03, 0.05); + EXPECT_EQ(5, search_parameters.num_angular_perturbations); + EXPECT_NEAR(0.03, search_parameters.angular_perturbation_step_size, 1e-9); + EXPECT_NEAR(0.05, search_parameters.resolution, 1e-9); + EXPECT_EQ(11, search_parameters.num_scans); + EXPECT_EQ(11, search_parameters.linear_bounds.size()); + for (const SearchParameters::LinearBounds linear_bounds : + search_parameters.linear_bounds) { + EXPECT_EQ(-4, linear_bounds.min_x); + EXPECT_EQ(4, linear_bounds.max_x); + EXPECT_EQ(-4, linear_bounds.min_y); + EXPECT_EQ(4, linear_bounds.max_y); + } +} + +TEST(Candidate, Construction) { + const SearchParameters search_parameters(4, 5, 0.03, 0.05); + const Candidate candidate(3, 4, -5, search_parameters); + EXPECT_EQ(3, candidate.scan_index); + EXPECT_EQ(4, candidate.x_index_offset); + EXPECT_EQ(-5, candidate.y_index_offset); + EXPECT_NEAR(0.25, candidate.x, 1e-9); + EXPECT_NEAR(-0.2, candidate.y, 1e-9); + EXPECT_NEAR(-0.06, candidate.orientation, 1e-9); + EXPECT_NEAR(0., candidate.score, 1e-9); + + Candidate bigger_candidate(3, 4, 5, search_parameters); + bigger_candidate.score = 1.; + EXPECT_LT(candidate, bigger_candidate); +} + +TEST(GenerateRotatedScans, GenerateRotatedScans) { + sensor::PointCloud2D point_cloud; + point_cloud.emplace_back(-1., 1.); + const std::vector scans = + GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.)); + EXPECT_EQ(3, scans.size()); + EXPECT_NEAR(1., scans[0][0].x(), 1e-6); + EXPECT_NEAR(1., scans[0][0].y(), 1e-6); + EXPECT_NEAR(-1., scans[1][0].x(), 1e-6); + EXPECT_NEAR(1., scans[1][0].y(), 1e-6); + EXPECT_NEAR(-1., scans[2][0].x(), 1e-6); + EXPECT_NEAR(-1., scans[2][0].y(), 1e-6); +} + +TEST(DiscretizeScans, DiscretizeScans) { + sensor::PointCloud2D point_cloud; + point_cloud.emplace_back(0.025, 0.175); + point_cloud.emplace_back(-0.025, 0.175); + point_cloud.emplace_back(-0.075, 0.175); + point_cloud.emplace_back(-0.125, 0.175); + point_cloud.emplace_back(-0.125, 0.125); + point_cloud.emplace_back(-0.125, 0.075); + point_cloud.emplace_back(-0.125, 0.025); + const MapLimits map_limits( + 0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025), + Eigen::Vector2d(0.025, 0.225))); + const std::vector scans = + GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = + DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity()); + EXPECT_EQ(1, discrete_scans.size()); + EXPECT_EQ(7, discrete_scans[0].size()); + EXPECT_TRUE((Eigen::Array2i(1, 0) == discrete_scans[0][0]).all()); + EXPECT_TRUE((Eigen::Array2i(1, 1) == discrete_scans[0][1]).all()); + EXPECT_TRUE((Eigen::Array2i(1, 2) == discrete_scans[0][2]).all()); + EXPECT_TRUE((Eigen::Array2i(1, 3) == discrete_scans[0][3]).all()); + EXPECT_TRUE((Eigen::Array2i(2, 3) == discrete_scans[0][4]).all()); + EXPECT_TRUE((Eigen::Array2i(3, 3) == discrete_scans[0][5]).all()); + EXPECT_TRUE((Eigen::Array2i(4, 3) == discrete_scans[0][6]).all()); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc new file mode 100644 index 0000000..d554b4e --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc @@ -0,0 +1,412 @@ +/* + * 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/scan_matching/fast_correlative_scan_matcher.h" + +#include +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +namespace { + +// A collection of values which can be added and later removed, and the maximum +// of the current values in the collection can be retrieved. +// All of it in (amortized) O(1). +class SlidingWindowMaximum { + public: + void AddValue(const float value) { + while (!non_ascending_maxima_.empty() && + value > non_ascending_maxima_.back()) { + non_ascending_maxima_.pop_back(); + } + non_ascending_maxima_.push_back(value); + } + + void RemoveValue(const float value) { + // DCHECK for performance, since this is done for every value in the + // precomputation grid. + DCHECK(!non_ascending_maxima_.empty()); + DCHECK_LE(value, non_ascending_maxima_.front()); + if (value == non_ascending_maxima_.front()) { + non_ascending_maxima_.pop_front(); + } + } + + float GetMaximum() const { + // DCHECK for performance, since this is done for every value in the + // precomputation grid. + DCHECK_GT(non_ascending_maxima_.size(), 0); + return non_ascending_maxima_.front(); + } + + void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(), 0); } + + private: + // Maximum of the current sliding window at the front. Then the maximum of the + // remaining window that came after this values first occurence, and so on. + std::deque non_ascending_maxima_; +}; + +} // namespace + +proto::FastCorrelativeScanMatcherOptions +CreateFastCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::FastCorrelativeScanMatcherOptions options; + options.set_linear_search_window( + parameter_dictionary->GetDouble("linear_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + options.set_branch_and_bound_depth( + parameter_dictionary->GetInt("branch_and_bound_depth")); + options.set_covariance_scale( + parameter_dictionary->GetDouble("covariance_scale")); + return options; +} + +PrecomputationGrid::PrecomputationGrid( + const ProbabilityGrid& probability_grid, const CellLimits& limits, + const int width, std::vector* reusable_intermediate_grid) + : offset_(-width + 1, -width + 1), + wide_limits_(limits.num_x_cells + width - 1, + limits.num_y_cells + width - 1), + cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) { + CHECK_GE(width, 1); + CHECK_GE(limits.num_x_cells, 1); + CHECK_GE(limits.num_y_cells, 1); + const int stride = wide_limits_.num_x_cells; + // First we compute the maximum probability for each (x0, y) achieved in the + // span defined by x0 <= x < x0 + width. + std::vector& intermediate = *reusable_intermediate_grid; + intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells); + for (int y = 0; y != limits.num_y_cells; ++y) { + SlidingWindowMaximum current_values; + current_values.AddValue( + probability_grid.GetProbability(Eigen::Array2i(0, y))); + for (int x = -width + 1; x != 0; ++x) { + intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); + if (x + width < limits.num_x_cells) { + current_values.AddValue( + probability_grid.GetProbability(Eigen::Array2i(x + width, y))); + } + } + for (int x = 0; x < limits.num_x_cells - width; ++x) { + intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); + current_values.RemoveValue( + probability_grid.GetProbability(Eigen::Array2i(x, y))); + current_values.AddValue( + probability_grid.GetProbability(Eigen::Array2i(x + width, y))); + } + for (int x = std::max(limits.num_x_cells - width, 0); + x != limits.num_x_cells; ++x) { + intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); + current_values.RemoveValue( + probability_grid.GetProbability(Eigen::Array2i(x, y))); + } + current_values.CheckIsEmpty(); + } + // For each (x, y), we compute the maximum probability in the width x width + // region starting at each (x, y) and precompute the resulting bound on the + // score. + for (int x = 0; x != wide_limits_.num_x_cells; ++x) { + SlidingWindowMaximum current_values; + current_values.AddValue(intermediate[x]); + for (int y = -width + 1; y != 0; ++y) { + cells_[x + (y + width - 1) * stride] = + ComputeCellValue(current_values.GetMaximum()); + if (y + width < limits.num_y_cells) { + current_values.AddValue(intermediate[x + (y + width) * stride]); + } + } + for (int y = 0; y < limits.num_y_cells - width; ++y) { + cells_[x + (y + width - 1) * stride] = + ComputeCellValue(current_values.GetMaximum()); + current_values.RemoveValue(intermediate[x + y * stride]); + current_values.AddValue(intermediate[x + (y + width) * stride]); + } + for (int y = std::max(limits.num_y_cells - width, 0); + y != limits.num_y_cells; ++y) { + cells_[x + (y + width - 1) * stride] = + ComputeCellValue(current_values.GetMaximum()); + current_values.RemoveValue(intermediate[x + y * stride]); + } + current_values.CheckIsEmpty(); + } +} + +uint8 PrecomputationGrid::ComputeCellValue(const float probability) const { + const int cell_value = common::RoundToInt( + (probability - mapping::kMinProbability) * + (255.f / (mapping::kMaxProbability - mapping::kMinProbability))); + CHECK_GE(cell_value, 0); + CHECK_LE(cell_value, 255); + return cell_value; +} + +class PrecomputationGridStack { + public: + PrecomputationGridStack( + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions& options) { + CHECK_GE(options.branch_and_bound_depth(), 1); + const int max_width = 1 << (options.branch_and_bound_depth() - 1); + precomputation_grids_.reserve(options.branch_and_bound_depth()); + std::vector reusable_intermediate_grid; + const CellLimits limits = probability_grid.limits().cell_limits(); + reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) * + limits.num_y_cells); + for (int i = 0; i != options.branch_and_bound_depth(); ++i) { + const int width = 1 << i; + precomputation_grids_.emplace_back(probability_grid, limits, width, + &reusable_intermediate_grid); + } + } + + const PrecomputationGrid& Get(int index) { + return precomputation_grids_[index]; + } + + int max_depth() const { return precomputation_grids_.size() - 1; } + + private: + std::vector precomputation_grids_; +}; + +FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions& options) + : options_(options), + limits_(probability_grid.limits()), + precomputation_grid_stack_( + new PrecomputationGridStack(probability_grid, options)) {} + +FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} + +bool FastCorrelativeScanMatcher::Match( + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, const float min_score, + float* score, transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const { + const SearchParameters search_parameters(options_.linear_search_window(), + options_.angular_search_window(), + point_cloud, limits_.resolution()); + return MatchWithSearchParameters(search_parameters, initial_pose_estimate, + point_cloud, min_score, score, pose_estimate, + covariance); +} + +bool FastCorrelativeScanMatcher::MatchFullSubmap( + const sensor::PointCloud2D& point_cloud, float min_score, float* score, + transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const { + // Compute a search window around the center of the submap that includes it + // fully. + const SearchParameters search_parameters( + 1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction. + M_PI, // Angular search window, 180 degrees in both directions. + point_cloud, limits_.resolution()); + const transform::Rigid2d center = + transform::Rigid2d::Translation(limits_.edge_limits().center()); + return MatchWithSearchParameters(search_parameters, center, point_cloud, + min_score, score, pose_estimate, covariance); +} + +bool FastCorrelativeScanMatcher::MatchWithSearchParameters( + SearchParameters search_parameters, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, float min_score, float* score, + transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const { + CHECK_NOTNULL(score); + CHECK_NOTNULL(pose_estimate); + CHECK_NOTNULL(covariance); + + const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); + const sensor::PointCloud2D rotated_point_cloud = + sensor::TransformPointCloud2D( + point_cloud, + transform::Rigid2d::Rotation(initial_rotation).cast()); + const std::vector rotated_scans = + GenerateRotatedScans(rotated_point_cloud, search_parameters); + const std::vector discrete_scans = DiscretizeScans( + limits_, rotated_scans, + Eigen::Translation2f(initial_pose_estimate.translation().x(), + initial_pose_estimate.translation().y())); + search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits()); + + const std::vector lowest_resolution_candidates = + ComputeLowestResolutionCandidates(discrete_scans, search_parameters); + // The following are intermediate results for computing covariance. See + // "Real-Time Correlative Scan Matching" by Olson. + Eigen::Matrix3d K = Eigen::Matrix3d::Zero(); + Eigen::Vector3d u = Eigen::Vector3d::Zero(); + double s = 0.; + const Candidate best_candidate = BranchAndBound( + discrete_scans, search_parameters, lowest_resolution_candidates, + precomputation_grid_stack_->max_depth(), min_score, &K, &u, &s); + if (best_candidate.score > min_score) { + *score = best_candidate.score; + *pose_estimate = transform::Rigid2d( + {initial_pose_estimate.translation().x() + best_candidate.x, + initial_pose_estimate.translation().y() + best_candidate.y}, + initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); + *covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose()); + *covariance *= options_.covariance_scale(); + return true; + } + return false; +} + +std::vector +FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( + const std::vector& discrete_scans, + const SearchParameters& search_parameters) const { + std::vector lowest_resolution_candidates = + GenerateLowestResolutionCandidates(search_parameters); + ScoreCandidates( + precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()), + discrete_scans, search_parameters, &lowest_resolution_candidates); + return lowest_resolution_candidates; +} + +std::vector +FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( + const SearchParameters& search_parameters) const { + const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); + int num_candidates = 0; + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + const int num_lowest_resolution_linear_x_candidates = + (search_parameters.linear_bounds[scan_index].max_x - + search_parameters.linear_bounds[scan_index].min_x + linear_step_size) / + linear_step_size; + const int num_lowest_resolution_linear_y_candidates = + (search_parameters.linear_bounds[scan_index].max_y - + search_parameters.linear_bounds[scan_index].min_y + linear_step_size) / + linear_step_size; + num_candidates += num_lowest_resolution_linear_x_candidates * + num_lowest_resolution_linear_y_candidates; + } + std::vector candidates; + candidates.reserve(num_candidates); + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x; + x_index_offset <= search_parameters.linear_bounds[scan_index].max_x; + x_index_offset += linear_step_size) { + for (int y_index_offset = + search_parameters.linear_bounds[scan_index].min_y; + y_index_offset <= search_parameters.linear_bounds[scan_index].max_y; + y_index_offset += linear_step_size) { + candidates.emplace_back(scan_index, x_index_offset, y_index_offset, + search_parameters); + } + } + } + CHECK_EQ(candidates.size(), num_candidates); + return candidates; +} + +void FastCorrelativeScanMatcher::ScoreCandidates( + const PrecomputationGrid& precomputation_grid, + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* const candidates) const { + for (Candidate& candidate : *candidates) { + int sum = 0; + for (const Eigen::Array2i& xy_index : + discrete_scans[candidate.scan_index]) { + const Eigen::Array2i proposed_xy_index( + xy_index.x() + candidate.x_index_offset, + xy_index.y() + candidate.y_index_offset); + sum += precomputation_grid.GetValue(proposed_xy_index); + } + candidate.score = PrecomputationGrid::ToProbability( + sum / static_cast(discrete_scans[candidate.scan_index].size())); + } + std::sort(candidates->begin(), candidates->end(), std::greater()); +} + +Candidate FastCorrelativeScanMatcher::BranchAndBound( + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + const std::vector& candidates, const int candidate_depth, + float min_score, Eigen::Matrix3d* K, Eigen::Vector3d* u, double* s) const { + if (candidate_depth == 0) { + // Update the covariance computation intermediate values. + for (const Candidate& candidate : candidates) { + const double p = candidate.score; + const Eigen::Vector3d xi(candidate.x, candidate.y, candidate.orientation); + *K += (xi * xi.transpose()) * p; + *u += xi * p; + *s += p; + } + // Return the best candidate. + return *candidates.begin(); + } + + Candidate best_high_resolution_candidate(0, 0, 0, search_parameters); + best_high_resolution_candidate.score = min_score; + for (const Candidate& candidate : candidates) { + if (candidate.score <= min_score) { + break; + } + std::vector higher_resolution_candidates; + const int half_width = 1 << (candidate_depth - 1); + for (int x_offset : {0, half_width}) { + if (candidate.x_index_offset + x_offset > + search_parameters.linear_bounds[candidate.scan_index].max_x) { + break; + } + for (int y_offset : {0, half_width}) { + if (candidate.y_index_offset + y_offset > + search_parameters.linear_bounds[candidate.scan_index].max_y) { + break; + } + higher_resolution_candidates.emplace_back( + candidate.scan_index, candidate.x_index_offset + x_offset, + candidate.y_index_offset + y_offset, search_parameters); + } + } + ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1), + discrete_scans, search_parameters, + &higher_resolution_candidates); + best_high_resolution_candidate = std::max( + best_high_resolution_candidate, + BranchAndBound(discrete_scans, search_parameters, + higher_resolution_candidates, candidate_depth - 1, + best_high_resolution_candidate.score, K, u, s)); + } + return best_high_resolution_candidate; +} + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h new file mode 100644 index 0000000..e39eca7 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h @@ -0,0 +1,162 @@ +/* + * 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. + */ + +// This is an implementation of the algorithm described in "Real-Time +// Correlative Scan Matching" by Olson. +// +// It is similar to the RealTimeCorrelativeScanMatcher but has a different +// trade-off: Scan matching is faster because more effort is put into the +// precomputation done for a given map. However, this map is immutable after +// construction. + +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/port.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +proto::FastCorrelativeScanMatcherOptions +CreateFastCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* parameter_dictionary); + +// A precomputed grid that contains in each cell (x0, y0) the maximum +// probability in the width x width area defined by x0 <= x < x0 + width and +// y0 <= y < y0. +class PrecomputationGrid { + public: + PrecomputationGrid(const ProbabilityGrid& probability_grid, + const CellLimits& limits, int width, + std::vector* reusable_intermediate_grid); + + // Returns a value between 0 and 255 to represent probabilities between + // kMinProbability and kMaxProbability. + int GetValue(const Eigen::Array2i& xy_index) const { + const Eigen::Array2i local_xy_index = xy_index - offset_; + // The static_cast is for performance to check with 2 comparisons + // xy_index.x() < offset_.x() || xy_index.y() < offset_.y() || + // local_xy_index.x() >= wide_limits_.num_x_cells || + // local_xy_index.y() >= wide_limits_.num_y_cells + // instead of using 4 comparisons. + if (static_cast(local_xy_index.x()) >= + static_cast(wide_limits_.num_x_cells) || + static_cast(local_xy_index.y()) >= + static_cast(wide_limits_.num_y_cells)) { + return 0; + } + const int stride = wide_limits_.num_x_cells; + return cells_[local_xy_index.x() + local_xy_index.y() * stride]; + } + + // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. + static float ToProbability(float value) { + return mapping::kMinProbability + + value * + ((mapping::kMaxProbability - mapping::kMinProbability) / 255.f); + } + + private: + uint8 ComputeCellValue(float probability) const; + + // Offset of the precomputation grid in relation to the 'probability_grid' + // including the additional 'width' - 1 cells. + const Eigen::Array2i offset_; + + // Size of the precomputation grid. + const CellLimits wide_limits_; + + // Probabilites mapped to 0 to 255. + std::vector cells_; +}; + +class PrecomputationGridStack; + +// An implementation of "Real-Time Correlative Scan Matching" by Olson. +class FastCorrelativeScanMatcher { + public: + FastCorrelativeScanMatcher( + const ProbabilityGrid& probability_grid, + const proto::FastCorrelativeScanMatcherOptions& options); + ~FastCorrelativeScanMatcher(); + + FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete; + FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) = + delete; + + // Aligns 'point_cloud' within the 'probability_grid' given an + // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) + // is possible, true is returned, and 'score', 'pose_estimate' and + // 'covariance' are updated with the result. + bool Match(const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, float min_score, + float* score, transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const; + + // Aligns 'point_cloud' within the full 'probability_grid', i.e., not + // restricted to the configured search window. If a score above 'min_score' + // (excluding equality) is possible, true is returned, and 'score', + // 'pose_estimate' and 'covariance' are updated with the result. + bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score, + float* score, transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const; + + private: + // The actual implementation of the scan matcher, called by Match() and + // MatchFullSubmap() with appropriate 'initial_pose_estimate' and + // 'search_parameters'. + bool MatchWithSearchParameters( + SearchParameters search_parameters, + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, float min_score, float* score, + transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const; + std::vector ComputeLowestResolutionCandidates( + const std::vector& discrete_scans, + const SearchParameters& search_parameters) const; + std::vector GenerateLowestResolutionCandidates( + const SearchParameters& search_parameters) const; + void ScoreCandidates(const PrecomputationGrid& precomputation_grid, + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* const candidates) const; + Candidate BranchAndBound(const std::vector& discrete_scans, + const SearchParameters& search_parameters, + const std::vector& candidates, + int candidate_depth, float min_score, + Eigen::Matrix3d* K, Eigen::Vector3d* u, + double* s) const; + + const proto::FastCorrelativeScanMatcherOptions options_; + MapLimits limits_; + std::unique_ptr precomputation_grid_stack_; +}; + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc new file mode 100644 index 0000000..b5c5cea --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -0,0 +1,229 @@ +/* + * 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/scan_matching/fast_correlative_scan_matcher.h" + +#include +#include +#include +#include +#include + +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { +namespace { + +TEST(PrecomputationGridTest, CorrectValues) { + // Create a probability grid with random values that can be exactly + // represented by uint8 values. + std::mt19937 prng(42); + std::uniform_int_distribution distribution(0, 255); + ProbabilityGrid probability_grid( + MapLimits(0.05, 5., 5., CellLimits(250, 250))); + probability_grid.StartUpdate(); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(probability_grid.limits().cell_limits(), + Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { + probability_grid.SetProbability( + xy_index, PrecomputationGrid::ToProbability(distribution(prng))); + } + + std::vector reusable_intermediate_grid; + for (const int width : {1, 2, 3, 8}) { + PrecomputationGrid precomputation_grid( + probability_grid, probability_grid.limits().cell_limits(), width, + &reusable_intermediate_grid); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + float max_score = -std::numeric_limits::infinity(); + for (int y = 0; y != width; ++y) { + for (int x = 0; x != width; ++x) { + max_score = std::max( + max_score, + probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); + } + } + EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( + precomputation_grid.GetValue(xy_index)), + 1e-4); + } + } +} + +TEST(PrecomputationGridTest, TinyProbabilityGrid) { + std::mt19937 prng(42); + std::uniform_int_distribution distribution(0, 255); + ProbabilityGrid probability_grid(MapLimits(0.05, 0.1, 0.1, CellLimits(4, 4))); + probability_grid.StartUpdate(); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + probability_grid.SetProbability( + xy_index, PrecomputationGrid::ToProbability(distribution(prng))); + } + + std::vector reusable_intermediate_grid; + for (const int width : {1, 2, 3, 8, 200}) { + PrecomputationGrid precomputation_grid( + probability_grid, probability_grid.limits().cell_limits(), width, + &reusable_intermediate_grid); + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(probability_grid.limits().cell_limits())) { + float max_score = -std::numeric_limits::infinity(); + for (int y = 0; y != width; ++y) { + for (int x = 0; x != width; ++x) { + max_score = std::max( + max_score, + probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); + } + } + EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( + precomputation_grid.GetValue(xy_index)), + 1e-4); + } + } +} + +proto::FastCorrelativeScanMatcherOptions +CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { + auto parameter_dictionary = + common::MakeDictionary(R"text( + return { + linear_search_window = 3., + angular_search_window = 1., + covariance_scale = 1., + branch_and_bound_depth = )text" + + std::to_string(branch_and_bound_depth) + "}"); + return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); +} + +mapping_2d::proto::LaserFanInserterOptions CreateLaserFanInserterTestOptions() { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + insert_free_space = true, + hit_probability = 0.7, + miss_probability = 0.4, + })text"); + return mapping_2d::CreateLaserFanInserterOptions(parameter_dictionary.get()); +} + +TEST(FastCorrelativeScanMatcherTest, CorrectPose) { + std::mt19937 prng(42); + std::uniform_real_distribution distribution(-1.f, 1.f); + LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); + constexpr float kMinScore = 0.1f; + const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); + + sensor::PointCloud2D point_cloud; + point_cloud.emplace_back(-2.5f, 0.5f); + point_cloud.emplace_back(-2.f, 0.5f); + point_cloud.emplace_back(0.f, -0.5f); + point_cloud.emplace_back(0.5f, -1.6f); + point_cloud.emplace_back(2.5f, 0.5f); + point_cloud.emplace_back(2.5f, 1.7f); + + for (int i = 0; i != 50; ++i) { + const transform::Rigid2f expected_pose( + {2. * distribution(prng), 2. * distribution(prng)}, + 0.5 * distribution(prng)); + + ProbabilityGrid probability_grid( + MapLimits(0.05, 5., 5., CellLimits(200, 200))); + probability_grid.StartUpdate(); + laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(), + sensor::TransformPointCloud2D( + point_cloud, expected_pose), + {}}, + &probability_grid); + + FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, + options); + transform::Rigid2d pose_estimate; + kalman_filter::Pose2DCovariance unused_covariance; + float score; + EXPECT_TRUE(fast_correlative_scan_matcher.Match( + transform::Rigid2d::Identity(), point_cloud, kMinScore, &score, + &pose_estimate, &unused_covariance)); + EXPECT_LT(kMinScore, score); + EXPECT_THAT(expected_pose, + transform::IsNearly(pose_estimate.cast(), 0.03f)) + << "Actual: " << transform::ToProto(pose_estimate).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + } +} + +TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { + std::mt19937 prng(42); + std::uniform_real_distribution distribution(-1.f, 1.f); + LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); + constexpr float kMinScore = 0.1f; + const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); + + sensor::PointCloud2D unperturbed_point_cloud; + unperturbed_point_cloud.emplace_back(-2.5, 0.5); + unperturbed_point_cloud.emplace_back(-2.25, 0.5); + unperturbed_point_cloud.emplace_back(0., 0.5); + unperturbed_point_cloud.emplace_back(0.25, 1.6); + unperturbed_point_cloud.emplace_back(2.5, 0.5); + unperturbed_point_cloud.emplace_back(2.0, 1.8); + + for (int i = 0; i != 20; ++i) { + const transform::Rigid2f perturbation( + {10. * distribution(prng), 10. * distribution(prng)}, + 1.6 * distribution(prng)); + const sensor::PointCloud2D point_cloud = + sensor::TransformPointCloud2D(unperturbed_point_cloud, perturbation); + const transform::Rigid2f expected_pose = + transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)}, + 0.5 * distribution(prng)) * + perturbation.inverse(); + + ProbabilityGrid probability_grid( + MapLimits(0.05, 5., 5., CellLimits(200, 200))); + probability_grid.StartUpdate(); + laser_fan_inserter.Insert( + sensor::LaserFan{ + (expected_pose * perturbation).translation(), + sensor::TransformPointCloud2D(point_cloud, expected_pose), + {}}, + &probability_grid); + + FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, + options); + transform::Rigid2d pose_estimate; + kalman_filter::Pose2DCovariance unused_covariance; + float score; + EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap( + point_cloud, kMinScore, &score, &pose_estimate, &unused_covariance)); + EXPECT_LT(kMinScore, score); + EXPECT_THAT(expected_pose, + transform::IsNearly(pose_estimate.cast(), 0.03f)) + << "Actual: " << transform::ToProto(pose_estimate).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc b/cartographer/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc new file mode 100644 index 0000000..b5f4120 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/fast_global_localizer.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_2d/scan_matching/fast_global_localizer.h" + +#include "cartographer/kalman_filter/pose_tracker.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +bool PerformGlobalLocalization( + const float cutoff, + const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, + const std::vector< + cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& + matchers, + const cartographer::sensor::PointCloud2D& point_cloud, + transform::Rigid2d* const best_pose_estimate, float* const best_score) { + CHECK(best_pose_estimate != nullptr) + << "Need a non-null output_pose_estimate!"; + CHECK(best_score != nullptr) << "Need a non-null best_score!"; + *best_score = cutoff; + transform::Rigid2d pose_estimate; + kalman_filter::Pose2DCovariance best_pose_estimate_covariance; + const sensor::PointCloud2D filtered_point_cloud = + voxel_filter.Filter(point_cloud); + bool success = false; + if (matchers.size() == 0) { + LOG(WARNING) << "Map not yet large enough to localize in!"; + return false; + } + for (auto& matcher : matchers) { + float score = -1; + transform::Rigid2d pose_estimate; + kalman_filter::Pose2DCovariance pose_estimate_covariance; + if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score, + &pose_estimate, &pose_estimate_covariance)) { + CHECK_GT(score, *best_score) << "MatchFullSubmap lied!"; + *best_score = score; + *best_pose_estimate = pose_estimate; + best_pose_estimate_covariance = pose_estimate_covariance; + success = true; + } + } + return success; +} + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/fast_global_localizer.h b/cartographer/cartographer/mapping_2d/scan_matching/fast_global_localizer.h new file mode 100644 index 0000000..e7bee48 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/fast_global_localizer.h @@ -0,0 +1,51 @@ +/* + * 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_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ + +#include + +#include "Eigen/Geometry" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/sensor/voxel_filter.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +// Perform global localization against the provided 'matchers'. The 'cutoff' +// specifies the minimum correlation that will be accepted. +// This function does not take ownership of the pointers passed in +// 'matchers'; they are passed as a vector of raw pointers to give maximum +// flexibility to callers. +// +// Returns true in the case of successful localization. The output parameters +// should not be trusted if the function returns false. The 'cutoff' and +// 'best_score' are in the range [0.0, 1.0]. +bool PerformGlobalLocalization( + float cutoff, const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, + const std::vector< + cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& + matchers, + const cartographer::sensor::PointCloud2D& point_cloud, + transform::Rigid2d* best_pose_estimate, float* best_score); + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h b/cartographer/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h new file mode 100644 index 0000000..baef23a --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "ceres/ceres.h" +#include "ceres/cubic_interpolation.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +// Computes the cost of inserting occupied space described by the point cloud +// into the map. The cost increases with the amount of free space that would be +// replaced by occupied space. +class OccupiedSpaceCostFunctor { + public: + // Creates an OccupiedSpaceCostFunctor using the specified map, resolution + // level, and point cloud. + OccupiedSpaceCostFunctor(const double scaling_factor, + const sensor::PointCloud2D& point_cloud, + const ProbabilityGrid& probability_grid) + : scaling_factor_(scaling_factor), + point_cloud_(point_cloud), + probability_grid_(probability_grid) {} + + OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; + OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; + + template + bool operator()(const T* const pose, T* residual) const { + Eigen::Matrix translation(pose[0], pose[1]); + Eigen::Rotation2D rotation(pose[2]); + Eigen::Matrix rotation_matrix = rotation.toRotationMatrix(); + Eigen::Matrix transform; + transform << rotation_matrix, translation, T(0.), T(0.), T(1.); + + const GridArrayAdapter adapter(probability_grid_); + ceres::BiCubicInterpolator interpolator(adapter); + const MapLimits& limits = probability_grid_.limits(); + + for (size_t i = 0; i < point_cloud_.size(); ++i) { + // Note that this is a 2D point. The third component is a scaling factor. + const Eigen::Matrix point((T(point_cloud_[i].x())), + (T(point_cloud_[i].y())), T(1.)); + const Eigen::Matrix world = transform * point; + interpolator.Evaluate((limits.centered_limits().max().x() - world[0]) / + limits.resolution() + + T(kPadding), + (limits.centered_limits().max().y() - world[1]) / + limits.resolution() + + T(kPadding), + &residual[i]); + residual[i] = scaling_factor_ * (1. - residual[i]); + } + return true; + } + + private: + static constexpr int kPadding = INT_MAX / 4; + class GridArrayAdapter { + public: + enum { DATA_DIMENSION = 1 }; + + explicit GridArrayAdapter(const ProbabilityGrid& probability_grid) + : probability_grid_(probability_grid) {} + + void GetValue(const int row, const int column, double* const value) const { + if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || + column >= NumCols() - kPadding) { + *value = mapping::kMinProbability; + } else { + *value = static_cast(probability_grid_.GetProbability( + Eigen::Array2i(column - kPadding, row - kPadding))); + } + } + + int NumRows() const { + return probability_grid_.limits().cell_limits().num_y_cells + + 2 * kPadding; + } + + int NumCols() const { + return probability_grid_.limits().cell_limits().num_x_cells + + 2 * kPadding; + } + + private: + const ProbabilityGrid& probability_grid_; + }; + + const double scaling_factor_; + const sensor::PointCloud2D& point_cloud_; + const ProbabilityGrid& probability_grid_; +}; + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/proto/CMakeLists.txt b/cartographer/cartographer/mapping_2d/scan_matching/proto/CMakeLists.txt new file mode 100644 index 0000000..b0eb4df --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/proto/CMakeLists.txt @@ -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. + +google_proto_library(mapping_2d_scan_matching_proto_ceres_scan_matcher_options + SRCS + ceres_scan_matcher_options.proto + DEPENDS + common_proto_ceres_solver_options +) + +google_proto_library(mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options + SRCS + fast_correlative_scan_matcher_options.proto +) + +google_proto_library(mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options + SRCS + real_time_correlative_scan_matcher_options.proto +) diff --git a/cartographer/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto new file mode 100644 index 0000000..cdda1d4 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto @@ -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. + +syntax = "proto2"; + +package cartographer.mapping_2d.scan_matching.proto; + +import "cartographer/common/proto/ceres_solver_options.proto"; + +// NEXT ID: 10 +message CeresScanMatcherOptions { + // Scaling parameters for each cost functor. + optional double occupied_space_cost_functor_weight = 1; + optional double previous_pose_translation_delta_cost_functor_weight = 2; + optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; + + // Scale applied to the covariance estimate from Ceres. + optional double covariance_scale = 4; + + // Configure the Ceres solver. See the Ceres documentation for more + // information: https://code.google.com/p/ceres-solver/ + optional common.proto.CeresSolverOptions ceres_solver_options = 9; +} diff --git a/cartographer/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto b/cartographer/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto new file mode 100644 index 0000000..b788269 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto @@ -0,0 +1,33 @@ +// 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.scan_matching.proto; + +message FastCorrelativeScanMatcherOptions { + // Minimum linear search window in which the best possible scan alignment + // will be found. + optional double linear_search_window = 3; + + // Minimum angular search window in which the best possible scan alignment + // will be found. + optional double angular_search_window = 4; + + // Number of precomputed grids to use. + optional int32 branch_and_bound_depth = 2; + + // Covariance estimate is multiplied by this scale. + optional double covariance_scale = 5; +}; diff --git a/cartographer/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto b/cartographer/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto new file mode 100644 index 0000000..8ea52bc --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto @@ -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. + +syntax = "proto2"; + +package cartographer.mapping_2d.scan_matching.proto; + +message RealTimeCorrelativeScanMatcherOptions { + // Minimum linear search window in which the best possible scan alignment + // will be found. + optional double linear_search_window = 1; + + // Minimum angular search window in which the best possible scan alignment + // will be found. + optional double angular_search_window = 2; + + // Weights applied to each part of the score. + optional double translation_delta_cost_weight = 3; + optional double rotation_delta_cost_weight = 4; + + // Covariance estimate is multiplied by this scale. + optional double covariance_scale = 5; +} diff --git a/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc new file mode 100644 index 0000000..a0930a0 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc @@ -0,0 +1,174 @@ +/* + * 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/scan_matching/real_time_correlative_scan_matcher.h" + +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::RealTimeCorrelativeScanMatcherOptions options; + options.set_linear_search_window( + parameter_dictionary->GetDouble("linear_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + options.set_translation_delta_cost_weight( + parameter_dictionary->GetDouble("translation_delta_cost_weight")); + options.set_rotation_delta_cost_weight( + parameter_dictionary->GetDouble("rotation_delta_cost_weight")); + options.set_covariance_scale( + parameter_dictionary->GetDouble("covariance_scale")); + CHECK_GE(options.translation_delta_cost_weight(), 0.); + CHECK_GE(options.rotation_delta_cost_weight(), 0.); + return options; +} + +RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( + const proto::RealTimeCorrelativeScanMatcherOptions& options) + : options_(options) {} + +std::vector +RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates( + const SearchParameters& search_parameters) const { + int num_candidates = 0; + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + const int num_linear_x_candidates = + (search_parameters.linear_bounds[scan_index].max_x - + search_parameters.linear_bounds[scan_index].min_x + 1); + const int num_linear_y_candidates = + (search_parameters.linear_bounds[scan_index].max_y - + search_parameters.linear_bounds[scan_index].min_y + 1); + num_candidates += num_linear_x_candidates * num_linear_y_candidates; + } + std::vector candidates; + candidates.reserve(num_candidates); + for (int scan_index = 0; scan_index != search_parameters.num_scans; + ++scan_index) { + for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x; + x_index_offset <= search_parameters.linear_bounds[scan_index].max_x; + ++x_index_offset) { + for (int y_index_offset = + search_parameters.linear_bounds[scan_index].min_y; + y_index_offset <= search_parameters.linear_bounds[scan_index].max_y; + ++y_index_offset) { + candidates.emplace_back(scan_index, x_index_offset, y_index_offset, + search_parameters); + } + } + } + CHECK_EQ(candidates.size(), num_candidates); + return candidates; +} + +double RealTimeCorrelativeScanMatcher::Match( + const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, + const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const { + CHECK_NOTNULL(pose_estimate); + CHECK_NOTNULL(covariance); + + const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); + const sensor::PointCloud2D rotated_point_cloud = + sensor::TransformPointCloud2D( + point_cloud, + transform::Rigid2d::Rotation(initial_rotation).cast()); + const SearchParameters search_parameters( + options_.linear_search_window(), options_.angular_search_window(), + rotated_point_cloud, probability_grid.limits().resolution()); + + const std::vector rotated_scans = + GenerateRotatedScans(rotated_point_cloud, search_parameters); + const std::vector discrete_scans = DiscretizeScans( + probability_grid.limits(), rotated_scans, + Eigen::Translation2f(initial_pose_estimate.translation().x(), + initial_pose_estimate.translation().y())); + std::vector candidates = + GenerateExhaustiveSearchCandidates(search_parameters); + ScoreCandidates(probability_grid, discrete_scans, search_parameters, + &candidates); + + // Covariance computed as in "Real-Time Correlative Scan Matching" by Olson. + Eigen::Matrix3d K = Eigen::Matrix3d::Zero(); + Eigen::Vector3d u = Eigen::Vector3d::Zero(); + double s = 0.f; + for (const Candidate& candidate : candidates) { + const double p = candidate.score; + const Eigen::Vector3d xi(candidate.x, candidate.y, candidate.orientation); + K += (xi * xi.transpose()) * p; + u += xi * p; + s += p; + } + *covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose()); + *covariance *= options_.covariance_scale(); + + const Candidate& best_candidate = + *std::max_element(candidates.begin(), candidates.end()); + *pose_estimate = transform::Rigid2d( + {initial_pose_estimate.translation().x() + best_candidate.x, + initial_pose_estimate.translation().y() + best_candidate.y}, + initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); + return best_candidate.score; +} + +void RealTimeCorrelativeScanMatcher::ScoreCandidates( + const ProbabilityGrid& probability_grid, + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* const candidates) const { + for (Candidate& candidate : *candidates) { + candidate.score = 0.f; + for (const Eigen::Array2i& xy_index : + discrete_scans[candidate.scan_index]) { + const Eigen::Array2i proposed_xy_index( + xy_index.x() + candidate.x_index_offset, + xy_index.y() + candidate.y_index_offset); + const float probability = + probability_grid.GetProbability(proposed_xy_index); + candidate.score += probability; + } + candidate.score /= + static_cast(discrete_scans[candidate.scan_index].size()); + candidate.score *= + std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) * + options_.translation_delta_cost_weight() + + std::abs(candidate.orientation) * + options_.rotation_delta_cost_weight())); + CHECK_GT(candidate.score, 0.f); + } +} + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h new file mode 100644 index 0000000..b7b8321 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.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. + */ + +// This is an implementation of the algorithm described in "Real-Time +// Correlative Scan Matching" by Olson. +// +// The correlative scan matching algorithm is exhaustively evaluating the scan +// matching search space. As described by the paper, the basic steps are: +// +// 1) Evaluate the probability p(z|xi, m) over the entire 3D search window using +// the low-resolution table. +// 2) Find the best voxel in the low-resolution 3D space that has not already +// been considered. Denote this value as Li. If Li < Hbest, terminate: Hbest is +// the best scan matching alignment. +// 3) Evaluate the search volume inside voxel i using the high resolution table. +// Suppose the log-likelihood of this voxel is Hi. Note that Hi <= Li since the +// low-resolution map overestimates the log likelihoods. If Hi > Hbest, set +// Hbest = Hi. +// +// This can be made even faster by transforming the scan exactly once over some +// discretized range. + +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ + +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +proto::RealTimeCorrelativeScanMatcherOptions +CreateRealTimeCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +// An implementation of "Real-Time Correlative Scan Matching" by Olson. +class RealTimeCorrelativeScanMatcher { + public: + explicit RealTimeCorrelativeScanMatcher( + const proto::RealTimeCorrelativeScanMatcherOptions& options); + + RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) = + delete; + RealTimeCorrelativeScanMatcher& operator=( + const RealTimeCorrelativeScanMatcher&) = delete; + + // Aligns 'point_cloud' within the 'probability_grid' given an + // 'initial_pose_estimate' then updates 'pose_estimate' and 'covariance' + // with the result and returns the score. + double Match(const transform::Rigid2d& initial_pose_estimate, + const sensor::PointCloud2D& point_cloud, + const ProbabilityGrid& probability_grid, + transform::Rigid2d* pose_estimate, + kalman_filter::Pose2DCovariance* covariance) const; + + // Computes the score for each Candidate in a collection. The cost is computed + // as the sum of probabilities, different from the Ceres CostFunctions: + // http://ceres-solver.org/modeling.html + // + // Visible for testing. + void ScoreCandidates(const ProbabilityGrid& probability_grid, + const std::vector& discrete_scans, + const SearchParameters& search_parameters, + std::vector* candidates) const; + + private: + std::vector GenerateExhaustiveSearchCandidates( + const SearchParameters& search_parameters) const; + + const proto::RealTimeCorrelativeScanMatcherOptions options_; +}; + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc new file mode 100644 index 0000000..3c1a04a --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -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. + */ + +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" + +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { +namespace { + +class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { + protected: + RealTimeCorrelativeScanMatcherTest() + : probability_grid_(MapLimits( + 0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025), + Eigen::Vector2d(0.025, 0.225)))) { + { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "insert_free_space = true, " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "}"); + laser_fan_inserter_ = common::make_unique( + CreateLaserFanInserterOptions(parameter_dictionary.get())); + } + point_cloud_.emplace_back(0.025, 0.175); + point_cloud_.emplace_back(-0.025, 0.175); + point_cloud_.emplace_back(-0.075, 0.175); + point_cloud_.emplace_back(-0.125, 0.175); + point_cloud_.emplace_back(-0.125, 0.125); + point_cloud_.emplace_back(-0.125, 0.075); + point_cloud_.emplace_back(-0.125, 0.025); + probability_grid_.StartUpdate(); + laser_fan_inserter_->Insert( + sensor::LaserFan{Eigen::Vector2f::Zero(), point_cloud_, {}}, + &probability_grid_); + { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "linear_search_window = 0.6, " + "angular_search_window = 0.16, " + "translation_delta_cost_weight = 0., " + "rotation_delta_cost_weight = 0., " + "covariance_scale = 1., " + "}"); + real_time_correlative_scan_matcher_ = + common::make_unique( + CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary.get())); + } + } + + ProbabilityGrid probability_grid_; + std::unique_ptr laser_fan_inserter_; + sensor::PointCloud2D point_cloud_; + std::unique_ptr + real_time_correlative_scan_matcher_; +}; + +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePerfectHighResolutionCandidate) { + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = DiscretizeScans( + probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), + &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(0, candidates[0].y_index_offset); + // Every point should align perfectly. + EXPECT_NEAR(0.7, candidates[0].score, 1e-2); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, + ScorePartiallyCorrectHighResolutionCandidate) { + const std::vector scans = + GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); + const std::vector discrete_scans = DiscretizeScans( + probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); + std::vector candidates; + candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); + real_time_correlative_scan_matcher_->ScoreCandidates( + probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), + &candidates); + EXPECT_EQ(0, candidates[0].scan_index); + EXPECT_EQ(0, candidates[0].x_index_offset); + EXPECT_EQ(1, candidates[0].y_index_offset); + // 3 points should align perfectly. + EXPECT_LT(0.7 * 3. / 7., candidates[0].score); + EXPECT_GT(0.7, candidates[0].score); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h b/cartographer/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h new file mode 100644 index 0000000..ff2a41b --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ + +#include "Eigen/Core" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +// Computes the cost of rotating the initial pose estimate. Cost increases with +// the solution's distance from the initial estimate. +class RotationDeltaCostFunctor { + public: + // Constructs a new RotationDeltaCostFunctor for the given 'angle'. + explicit RotationDeltaCostFunctor(const double scaling_factor, + const double angle) + : scaling_factor_(scaling_factor), angle_(angle) {} + + RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete; + RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete; + + template + bool operator()(const T* const pose, T* residual) const { + residual[0] = scaling_factor_ * (pose[2] - angle_); + return true; + } + + private: + const double scaling_factor_; + const double angle_; +}; + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ diff --git a/cartographer/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h b/cartographer/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h new file mode 100644 index 0000000..ce99860 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.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_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ +#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ + +#include "Eigen/Core" + +namespace cartographer { +namespace mapping_2d { +namespace scan_matching { + +// Computes the cost of translating the initial pose estimate. Cost increases +// with the solution's distance from the initial estimate. +class TranslationDeltaCostFunctor { + public: + // Constructs a new TranslationDeltaCostFunctor from the given + // 'initial_pose_estimate' (x, y, theta). + explicit TranslationDeltaCostFunctor( + const double scaling_factor, + const transform::Rigid2d& initial_pose_estimate) + : scaling_factor_(scaling_factor), + x_(initial_pose_estimate.translation().x()), + y_(initial_pose_estimate.translation().y()) {} + + TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; + TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = + delete; + + template + bool operator()(const T* const pose, T* residual) const { + residual[0] = scaling_factor_ * (pose[0] - x_); + residual[1] = scaling_factor_ * (pose[1] - y_); + return true; + } + + private: + const double scaling_factor_; + const double x_; + const double y_; +}; + +} // namespace scan_matching +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/cartographer/mapping_2d/sparse_pose_graph.cc new file mode 100644 index 0000000..f599ede --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph.cc @@ -0,0 +1,447 @@ +/* + * 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/sparse_pose_graph.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/scan_matching_progress.pb.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/sensor/voxel_filter.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { + +SparsePoseGraph::SparsePoseGraph( + const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool, + std::deque* constant_node_data) + : options_(options), + optimization_problem_(options_.optimization_problem_options()), + constraint_builder_(options_.constraint_builder_options(), thread_pool), + constant_node_data_(constant_node_data) {} + +SparsePoseGraph::~SparsePoseGraph() { + WaitForAllComputations(); + common::MutexLocker locker(&mutex_); + CHECK(scan_queue_ == nullptr); +} + +void SparsePoseGraph::GrowSubmapTransformsAsNeeded( + const std::vector& submaps) { + CHECK(!submaps.empty()); + CHECK_LT(submap_transforms_.size(), std::numeric_limits::max()); + const int next_transform_index = submap_transforms_.size(); + // Verify that we have an index for the first submap. + const int first_transform_index = GetSubmapIndex(submaps[0]); + if (submaps.size() == 1) { + // If we don't already have an entry for this submap, add one. + if (first_transform_index == next_transform_index) { + submap_transforms_.push_back(transform::Rigid2d::Identity()); + } + return; + } + CHECK_EQ(2, submaps.size()); + // CHECK that we have a index for the second submap. + const int second_transform_index = GetSubmapIndex(submaps[1]); + CHECK_LE(second_transform_index, next_transform_index); + // Extrapolate if necessary. + if (second_transform_index == next_transform_index) { + const auto& first_submap_transform = + submap_transforms_[first_transform_index]; + submap_transforms_.push_back( + first_submap_transform * + sparse_pose_graph::ComputeSubmapPose(*submaps[0]).inverse() * + sparse_pose_graph::ComputeSubmapPose(*submaps[1])); + } +} + +void SparsePoseGraph::AddScan( + common::Time time, const transform::Rigid3d& tracking_to_pose, + const sensor::LaserFan& laser_fan_in_pose, const transform::Rigid2d& pose, + const kalman_filter::Pose2DCovariance& covariance, + const mapping::Submaps* submaps, + const mapping::Submap* const matching_submap, + const std::vector& insertion_submaps) { + const transform::Rigid3d optimized_pose(GetOdometryToMapTransform(*submaps) * + transform::Embed3D(pose)); + + common::MutexLocker locker(&mutex_); + const int j = trajectory_nodes_.size(); + CHECK_LT(j, std::numeric_limits::max()); + + constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ + time, laser_fan_in_pose, + Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}), + submaps, transform::Rigid3d(tracking_to_pose)}); + trajectory_nodes_.push_back(mapping::TrajectoryNode{ + &constant_node_data_->back(), optimized_pose, + }); + trajectory_connectivity_.Add(submaps); + + if (submap_indices_.count(insertion_submaps.back()) == 0) { + submap_indices_.emplace(insertion_submaps.back(), + static_cast(submap_indices_.size())); + submap_states_.emplace_back(); + submap_states_.back().submap = insertion_submaps.back(); + submap_states_.back().trajectory = submaps; + CHECK_EQ(submap_states_.size(), submap_indices_.size()); + } + const mapping::Submap* const finished_submap = + insertion_submaps.front()->finished_probability_grid != nullptr + ? insertion_submaps.front() + : nullptr; + + // Make sure we have a sampler for this trajectory. + if (!global_localization_samplers_[submaps]) { + global_localization_samplers_[submaps] = + common::make_unique( + options_.global_sampling_ratio()); + } + AddWorkItem( + std::bind(std::mem_fn(&SparsePoseGraph::ComputeConstraintsForScan), this, + j, submaps, matching_submap, insertion_submaps, finished_submap, + pose, covariance)); +} + +void SparsePoseGraph::ComputeConstraintsForOldScans( + const mapping::Submap* submap) { + const int submap_index = GetSubmapIndex(submap); + CHECK_GT(point_cloud_poses_.size(), 0); + CHECK_LT(point_cloud_poses_.size(), std::numeric_limits::max()); + const int num_nodes = point_cloud_poses_.size(); + for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { + if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) { + const transform::Rigid2d relative_pose = + submap_transforms_[submap_index].inverse() * + point_cloud_poses_[scan_index]; + constraint_builder_.MaybeAddConstraint( + submap_index, submap, scan_index, + &trajectory_nodes_[scan_index].constant_data->laser_fan.point_cloud, + relative_pose); + } + } +} + +void SparsePoseGraph::ComputeConstraintsForScan( + int scan_index, const mapping::Submaps* scan_trajectory, + const mapping::Submap* matching_submap, + std::vector insertion_submaps, + const mapping::Submap* finished_submap, const transform::Rigid2d& pose, + const kalman_filter::Pose2DCovariance& covariance) { + GrowSubmapTransformsAsNeeded(insertion_submaps); + const int matching_index = GetSubmapIndex(matching_submap); + const transform::Rigid2d optimized_pose = + submap_transforms_[matching_index] * + sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; + CHECK_EQ(scan_index, point_cloud_poses_.size()); + initial_point_cloud_poses_.push_back(pose); + point_cloud_poses_.push_back(optimized_pose); + for (const mapping::Submap* submap : insertion_submaps) { + const int submap_index = GetSubmapIndex(submap); + CHECK(!submap_states_[submap_index].finished); + submap_states_[submap_index].scan_indices.emplace(scan_index); + // Unchanged covariance as (submap <- map) is a translation. + const transform::Rigid2d constraint_transform = + sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; + constraints_.push_back( + Constraint2D{submap_index, + scan_index, + {constraint_transform, + constraint_builder_.ComputeSqrtLambda(covariance)}, + Constraint2D::INTRA_SUBMAP}); + } + + // Determine if this scan should be globally localized. + const bool run_global_localization = + global_localization_samplers_[scan_trajectory]->Pulse(); + + CHECK_LT(submap_states_.size(), std::numeric_limits::max()); + const int num_submaps = submap_states_.size(); + for (int submap_index = 0; submap_index != num_submaps; ++submap_index) { + if (submap_states_[submap_index].finished) { + CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0); + const transform::Rigid2d relative_pose = + submap_transforms_[submap_index].inverse() * + point_cloud_poses_[scan_index]; + + const auto* submap_trajectory = submap_states_[submap_index].trajectory; + + // Only globally match against submaps not in this trajectory. + if (run_global_localization && scan_trajectory != submap_trajectory) { + constraint_builder_.MaybeAddGlobalConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + scan_trajectory, submap_trajectory, &trajectory_connectivity_, + &trajectory_nodes_[scan_index] + .constant_data->laser_fan.point_cloud); + } else { + const bool scan_and_submap_trajectories_connected = + reverse_connected_components_.count(scan_trajectory) > 0 && + reverse_connected_components_.count(submap_trajectory) > 0 && + reverse_connected_components_.at(scan_trajectory) == + reverse_connected_components_.at(submap_trajectory); + if (scan_trajectory == submap_trajectory || + scan_and_submap_trajectories_connected) { + constraint_builder_.MaybeAddConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + &trajectory_nodes_[scan_index] + .constant_data->laser_fan.point_cloud, + relative_pose); + } + } + } + } + + if (finished_submap != nullptr) { + const int finished_submap_index = GetSubmapIndex(finished_submap); + SubmapState& finished_submap_state = submap_states_[finished_submap_index]; + CHECK(!finished_submap_state.finished); + if (options_.also_match_to_new_submaps()) { + // We have a new completed submap, so we look into adding constraints for + // old scans. + ComputeConstraintsForOldScans(finished_submap); + } + finished_submap_state.finished = true; + } + constraint_builder_.NotifyEndOfScan(scan_index); + ++num_scans_since_last_loop_closure_; + if (options_.optimize_every_n_scans() > 0 && + num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { + CHECK(!run_loop_closure_); + run_loop_closure_ = true; + // If there is a 'scan_queue_' already, some other thread will take care. + if (scan_queue_ == nullptr) { + scan_queue_ = common::make_unique>>(); + HandleScanQueue(); + } + } +} + +void SparsePoseGraph::HandleScanQueue() { + constraint_builder_.WhenDone( + [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + RunOptimization(); + + common::MutexLocker locker(&mutex_); + num_scans_since_last_loop_closure_ = 0; + run_loop_closure_ = false; + while (!run_loop_closure_) { + if (scan_queue_->empty()) { + LOG(INFO) << "We caught up. Hooray!"; + scan_queue_.reset(); + return; + } + scan_queue_->front()(); + scan_queue_->pop_front(); + } + // We have to optimize again. + HandleScanQueue(); + }); +} + +void SparsePoseGraph::WaitForAllComputations() { + bool notification = false; + common::MutexLocker locker(&mutex_); + const int num_finished_scans_at_start = + constraint_builder_.GetNumFinishedScans(); + while (!locker.AwaitWithTimeout( + [this]() REQUIRES(mutex_) { + return static_cast(constraint_builder_.GetNumFinishedScans()) == + trajectory_nodes_.size(); + }, + common::FromSeconds(1.))) { + std::ostringstream progress_info; + progress_info << "Optimizing: " << std::fixed << std::setprecision(1) + << 100. * (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / + (trajectory_nodes_.size() - + num_finished_scans_at_start) + << "%..."; + std::cout << "\r\x1b[K" << progress_info.str() << std::flush; + } + std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; + constraint_builder_.WhenDone([this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + common::MutexLocker locker(&mutex_); + notification = true; + }); + locker.Await([¬ification]() { return notification; }); +} + +void SparsePoseGraph::AddWorkItem(std::function work_item) { + if (scan_queue_ == nullptr) { + work_item(); + } else { + scan_queue_->push_back(work_item); + } +} + +void SparsePoseGraph::RunFinalOptimization() { + WaitForAllComputations(); + optimization_problem_.SetMaxNumIterations( + options_.max_num_final_iterations()); + RunOptimization(); +} + +void SparsePoseGraph::RunOptimization() { + if (!submap_transforms_.empty()) { + std::vector trajectories; + { + common::MutexLocker locker(&mutex_); + CHECK(!submap_states_.empty()); + for (const auto& trajectory_node : trajectory_nodes_) { + trajectories.push_back(trajectory_node.constant_data->trajectory); + } + } + + optimization_problem_.Solve(constraints_, trajectories, + initial_point_cloud_poses_, &point_cloud_poses_, + &submap_transforms_); + common::MutexLocker locker(&mutex_); + has_new_optimized_poses_ = true; + const size_t num_optimized_poses = point_cloud_poses_.size(); + for (size_t i = 0; i != num_optimized_poses; ++i) { + trajectory_nodes_[i].pose = + transform::Rigid3d(transform::Embed3D(point_cloud_poses_[i])); + } + // Extrapolate all point cloud poses that were added later. + std::unordered_map + extrapolation_transforms; + for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { + const mapping::Submaps* trajectory = + trajectory_nodes_[i].constant_data->trajectory; + if (extrapolation_transforms.count(trajectory) == 0) { + extrapolation_transforms[trajectory] = transform::Rigid3d( + ExtrapolateSubmapTransforms(submap_transforms_, *trajectory) + .back() * + ExtrapolateSubmapTransforms(optimized_submap_transforms_, + *trajectory) + .back() + .inverse()); + } + trajectory_nodes_[i].pose = + extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; + } + optimized_submap_transforms_ = submap_transforms_; + connected_components_ = trajectory_connectivity_.ConnectedComponents(); + reverse_connected_components_.clear(); + for (size_t i = 0; i != connected_components_.size(); ++i) { + for (const auto* trajectory : connected_components_[i]) { + reverse_connected_components_.emplace(trajectory, i); + } + } + } +} + +bool SparsePoseGraph::HasNewOptimizedPoses() { + common::MutexLocker locker(&mutex_); + if (!has_new_optimized_poses_) { + return false; + } + has_new_optimized_poses_ = false; + return true; +} + +mapping::proto::ScanMatchingProgress +SparsePoseGraph::GetScanMatchingProgress() { + mapping::proto::ScanMatchingProgress progress; + common::MutexLocker locker(&mutex_); + progress.set_num_scans_total(trajectory_nodes_.size()); + progress.set_num_scans_finished(constraint_builder_.GetNumFinishedScans()); + return progress; +} + +std::vector SparsePoseGraph::GetTrajectoryNodes() { + common::MutexLocker locker(&mutex_); + return trajectory_nodes_; +} + +std::vector SparsePoseGraph::constraints_2d() { + return constraints_; +} + +std::vector SparsePoseGraph::constraints_3d() { + return {}; +} + +transform::Rigid3d SparsePoseGraph::GetOdometryToMapTransform( + const mapping::Submaps& submaps) { + return GetSubmapTransforms(submaps).back() * + submaps.Get(submaps.size() - 1)->local_pose().inverse(); +} + +std::vector> +SparsePoseGraph::GetConnectedTrajectories() { + common::MutexLocker locker(&mutex_); + return connected_components_; +} + +std::vector SparsePoseGraph::GetSubmapTransforms( + const mapping::Submaps& submaps) { + common::MutexLocker locker(&mutex_); + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps); +} + +std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( + const std::vector& submap_transforms, + const mapping::Submaps& submaps) const { + std::vector result; + int i = 0; + // Submaps for which we have optimized poses. + for (; i < submaps.size(); ++i) { + const mapping::Submap* submap = submaps.Get(i); + const auto submap_and_index = submap_indices_.find(submap); + // Determine if we have a loop-closed transform for this submap. + if (submap_and_index == submap_indices_.end() || + static_cast(submap_and_index->second) >= + submap_transforms.size()) { + break; + } + result.push_back( + transform::Embed3D(submap_transforms[submap_and_index->second])); + } + + // Extrapolate to the remaining submaps. + for (; i < submaps.size(); ++i) { + if (i == 0) { + result.push_back(transform::Rigid3d::Identity()); + continue; + } + result.push_back(result.back() * + submaps.Get(result.size() - 1)->local_pose().inverse() * + submaps.Get(result.size())->local_pose()); + } + return result; +} + +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/cartographer/mapping_2d/sparse_pose_graph.h new file mode 100644 index 0000000..2ef8bda --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph.h @@ -0,0 +1,215 @@ +/* + * 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. + */ + +// Implements the loop closure method called Sparse Pose Adjustment (SPA) from +// 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. +// +// It is extended for submapping: +// Each scan has been matched against one or more submaps (adding a constraint +// for each match), both poses of scans and of submaps are to be optimized. +// All constraints are between a submap i and a scan j. + +#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ +#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ + +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/proto/scan_matching_progress.pb.h" +#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/trajectory_connectivity.h" +#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" +#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" +#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping_2d { + +// Implements the SPA loop closure method. +class SparsePoseGraph : public mapping::SparsePoseGraph { + public: + SparsePoseGraph( + const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool, + std::deque* constant_node_data); + ~SparsePoseGraph() override; + + SparsePoseGraph(const SparsePoseGraph&) = delete; + SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; + + // Adds a new 'laser_fan_in_pose' observation at 'time', and a 'pose' + // that will later be optimized. The 'tracking_to_pose' is remembered so + // that the optimized pose can be embedded into 3D. The 'pose' was determined + // by scan matching against the 'matching_submap' and the scan was inserted + // into the 'insertion_submaps'. + void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose, + const sensor::LaserFan& laser_fan_in_pose, + const transform::Rigid2d& pose, + const kalman_filter::Pose2DCovariance& pose_covariance, + const mapping::Submaps* submaps, + const mapping::Submap* matching_submap, + const std::vector& insertion_submaps) + EXCLUDES(mutex_); + + void RunFinalOptimization() override; + bool HasNewOptimizedPoses() override; + mapping::proto::ScanMatchingProgress GetScanMatchingProgress() override; + + std::vector> GetConnectedTrajectories() + override; + std::vector GetSubmapTransforms( + const mapping::Submaps& submaps) EXCLUDES(mutex_) override; + transform::Rigid3d GetOdometryToMapTransform(const mapping::Submaps& submaps) + EXCLUDES(mutex_) override; + std::vector GetTrajectoryNodes() override + EXCLUDES(mutex_); + std::vector constraints_2d() override; + std::vector constraints_3d() override; + + private: + struct SubmapState { + const mapping::Submap* submap = nullptr; + + // Indices of the scans that were inserted into this map together with + // constraints for them. They are not to be matched again when this submap + // becomes 'finished'. + std::set scan_indices; + + // Whether in the current state of the background thread this submap is + // finished. When this transitions to true, all scans are tried to match + // against this submap. Likewise, all new scans are matched against submaps + // which are finished. + bool finished = false; + + // The trajectory to which this SubmapState belongs. + const mapping::Submaps* trajectory = nullptr; + }; + + int GetSubmapIndex(const mapping::Submap* submap) const REQUIRES(mutex_) { + const auto iterator = submap_indices_.find(submap); + CHECK(iterator != submap_indices_.end()); + return iterator->second; + } + + // Grows 'submap_transforms_' to have an entry for every element of 'submaps'. + void GrowSubmapTransformsAsNeeded( + const std::vector& submaps) REQUIRES(mutex_); + + // Adds constraints for a scan, and starts scan matching in the background. + void ComputeConstraintsForScan( + int scan_index, const mapping::Submaps* scan_trajectory, + const mapping::Submap* matching_submap, + std::vector insertion_submaps, + const mapping::Submap* finished_submap, const transform::Rigid2d& pose, + const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_); + + // Adds constraints for older scans whenever a new submap is finished. + void ComputeConstraintsForOldScans(const mapping::Submap* submap) + REQUIRES(mutex_); + + // Registers the callback to run the optimization once all constraints have + // been computed, that will also do all work that queue up in 'scan_queue_'. + void HandleScanQueue() REQUIRES(mutex_); + + // Waits until we caught up (i.e. nothing is waiting to be scheduled), and + // all computations have finished. + void WaitForAllComputations() EXCLUDES(mutex_); + + // Runs the optimization. Callers have to make sure, that there is only one + // optimization being run at a time. + void RunOptimization() EXCLUDES(mutex_); + + // Handles a new 'work_item'. + void AddWorkItem(std::function work_item) REQUIRES(mutex_); + + // Adds extrapolated transforms, so that there are transforms for all submaps. + std::vector ExtrapolateSubmapTransforms( + const std::vector& submap_transforms, + const mapping::Submaps& submaps) const REQUIRES(mutex_); + + const mapping::proto::SparsePoseGraphOptions options_; + common::Mutex mutex_; + + // If it exists, further scans must be added to this queue, and will be + // considered later. + std::unique_ptr>> scan_queue_ + GUARDED_BY(mutex_); + + // How our various trajectories are related. + mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); + + // We globally localize a fraction of the scans from each trajectory. + std::unordered_map> + global_localization_samplers_ GUARDED_BY(mutex_); + + // Number of scans added since last loop closure. + int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; + + // Whether the optimization has to be run before more data is added. + bool run_loop_closure_ GUARDED_BY(mutex_) = false; + + // Current optimization problem. + sparse_pose_graph::OptimizationProblem optimization_problem_; + sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); + std::vector constraints_; + std::vector initial_point_cloud_poses_; + std::vector point_cloud_poses_; // (map <- point cloud) + std::vector submap_transforms_; // (map <- submap) + + // Submaps get assigned an index and state as soon as they are seen, even + // before they take part in the background computations. + std::map submap_indices_ GUARDED_BY(mutex_); + std::vector submap_states_ GUARDED_BY(mutex_); + + // Whether to return true on the next call to HasNewOptimizedPoses(). + bool has_new_optimized_poses_ GUARDED_BY(mutex_) = false; + + // Connectivity structure of our trajectories. + std::vector> connected_components_; + // Trajectory to connected component ID. + std::map reverse_connected_components_; + + // Data that are currently being shown. + // + // Deque to keep references valid for the background computation when adding + // new data. + std::deque* constant_node_data_; + std::vector trajectory_nodes_ GUARDED_BY(mutex_); + + // Current submap transforms used for displaying data. + std::vector optimized_submap_transforms_ + GUARDED_BY(mutex_); +}; + +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt b/cartographer/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt new file mode 100644 index 0000000..8ef4f39 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt @@ -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. + +google_library(mapping_2d_sparse_pose_graph_constraint_builder + USES_CERES + USES_EIGEN + SRCS + constraint_builder.cc + HDRS + constraint_builder.h + DEPENDS + common_fixed_ratio_sampler + common_histogram + common_make_unique + common_math + common_mutex + common_thread_pool + kalman_filter_pose_tracker + mapping_2d_scan_matching_ceres_scan_matcher + mapping_2d_scan_matching_fast_correlative_scan_matcher + mapping_2d_scan_matching_proto_ceres_scan_matcher_options + mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options + mapping_2d_sparse_pose_graph_optimization_problem + mapping_2d_submaps + mapping_3d_scan_matching_ceres_scan_matcher + mapping_3d_scan_matching_fast_correlative_scan_matcher + mapping_sparse_pose_graph_proto_constraint_builder_options + mapping_trajectory_connectivity + sensor_point_cloud + sensor_voxel_filter + transform_transform +) + +google_library(mapping_2d_sparse_pose_graph_optimization_problem + USES_CERES + USES_EIGEN + SRCS + optimization_problem.cc + HDRS + optimization_problem.h + DEPENDS + common_ceres_solver_options + common_histogram + common_lua_parameter_dictionary + common_math + common_port + mapping_2d_submaps + mapping_sparse_pose_graph + mapping_sparse_pose_graph_proto_optimization_problem_options + transform_transform +) diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc new file mode 100644 index 0000000..dbde010 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -0,0 +1,309 @@ +/* + * 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/sparse_pose_graph/constraint_builder.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { +namespace sparse_pose_graph { + +transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) { + return transform::Project2D(submap.local_pose()); +} + +ConstraintBuilder::ConstraintBuilder( + const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, + common::ThreadPool* const thread_pool) + : options_(options), + thread_pool_(thread_pool), + sampler_(options.sampling_ratio()), + adaptive_voxel_filter_(options.adaptive_voxel_filter_options()), + ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} + +ConstraintBuilder::~ConstraintBuilder() { + common::MutexLocker locker(&mutex_); + CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; + CHECK_EQ(pending_computations_.size(), 0); + CHECK_EQ(submap_queued_work_items_.size(), 0); + CHECK(when_done_ == nullptr); +} + +void ConstraintBuilder::MaybeAddConstraint( + const int submap_index, const mapping::Submap* const submap, + const int scan_index, const sensor::PointCloud2D* const point_cloud, + const transform::Rigid2d& initial_relative_pose) { + if (initial_relative_pose.translation().norm() > + options_.max_constraint_distance()) { + return; + } + if (sampler_.Pulse()) { + common::MutexLocker locker(&mutex_); + CHECK_LE(scan_index, current_computation_); + constraints_.emplace_back(); + auto* const constraint = &constraints_.back(); + ++pending_computations_[current_computation_]; + const int current_computation = current_computation_; + ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + submap_index, submap->finished_probability_grid, + [=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_index, submap, scan_index, + nullptr, /* scan_trajectory */ + nullptr, /* submap_trajectory */ + false, /* match_full_submap */ + nullptr, /* trajectory_connectivity */ + point_cloud, initial_relative_pose, constraint); + FinishComputation(current_computation); + }); + } +} + +void ConstraintBuilder::MaybeAddGlobalConstraint( + const int submap_index, const mapping::Submap* const submap, + const int scan_index, const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const sensor::PointCloud2D* const point_cloud) { + common::MutexLocker locker(&mutex_); + CHECK_LE(scan_index, current_computation_); + constraints_.emplace_back(); + auto* const constraint = &constraints_.back(); + ++pending_computations_[current_computation_]; + const int current_computation = current_computation_; + ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + submap_index, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_index, submap, scan_index, submap_trajectory, + scan_trajectory, true, /* match_full_submap */ + trajectory_connectivity, point_cloud, + transform::Rigid2d::Identity(), constraint); + FinishComputation(current_computation); + }); +} + +void ConstraintBuilder::NotifyEndOfScan(const int scan_index) { + common::MutexLocker locker(&mutex_); + CHECK_EQ(current_computation_, scan_index); + ++current_computation_; +} + +void ConstraintBuilder::WhenDone( + const std::function callback) { + common::MutexLocker locker(&mutex_); + CHECK(when_done_ == nullptr); + when_done_ = + common::make_unique>(callback); + ++pending_computations_[current_computation_]; + const int current_computation = current_computation_; + thread_pool_->Schedule( + [this, current_computation] { FinishComputation(current_computation); }); +} + +Eigen::Matrix3d ConstraintBuilder::ComputeSqrtLambda( + const Eigen::Matrix3d& covariance) const { + return common::ComputeSpdMatrixSqrtInverse( + covariance, options_.max_covariance_trace(), + options_.lower_covariance_eigenvalue_bound()); +} + +void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + const int submap_index, const ProbabilityGrid* const submap, + const std::function work_item) { + if (submap_scan_matchers_[submap_index].fast_correlative_scan_matcher != + nullptr) { + thread_pool_->Schedule(work_item); + } else { + submap_queued_work_items_[submap_index].push_back(work_item); + if (submap_queued_work_items_[submap_index].size() == 1) { + thread_pool_->Schedule( + std::bind(std::mem_fn(&ConstraintBuilder::ConstructSubmapScanMatcher), + this, submap_index, submap)); + } + } +} + +void ConstraintBuilder::ConstructSubmapScanMatcher( + const int submap_index, const ProbabilityGrid* const submap) { + auto submap_scan_matcher = + common::make_unique( + *submap, options_.fast_correlative_scan_matcher_options()); + common::MutexLocker locker(&mutex_); + submap_scan_matchers_[submap_index] = {submap, + std::move(submap_scan_matcher)}; + for (const std::function& work_item : + submap_queued_work_items_[submap_index]) { + thread_pool_->Schedule(work_item); + } + submap_queued_work_items_.erase(submap_index); +} + +const ConstraintBuilder::SubmapScanMatcher* +ConstraintBuilder::GetSubmapScanMatcher(const int submap_index) { + common::MutexLocker locker(&mutex_); + const SubmapScanMatcher* submap_scan_matcher = + &submap_scan_matchers_[submap_index]; + CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr); + return submap_scan_matcher; +} + +void ConstraintBuilder::ComputeConstraint( + const int submap_index, const mapping::Submap* const submap, + const int scan_index, const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, bool match_full_submap, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const sensor::PointCloud2D* const point_cloud, + const transform::Rigid2d& initial_relative_pose, + std::unique_ptr* constraint) { + const transform::Rigid2d initial_pose = + ComputeSubmapPose(*submap) * initial_relative_pose; + const SubmapScanMatcher* const submap_scan_matcher = + GetSubmapScanMatcher(submap_index); + const sensor::PointCloud2D filtered_point_cloud = + adaptive_voxel_filter_.Filter(*point_cloud); + + // The 'constraint_transform' (i <- j) is computed from: + // - a 'filtered_point_cloud' in j, + // - the initial guess 'initial_pose' for (map <- j), + // - the result 'pose_estimate' of Match() (map <- j). + // - the ComputeSubmapPose() (map <- i) + float score = 0.; + transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); + kalman_filter::Pose2DCovariance covariance; + + if (match_full_submap) { + if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( + filtered_point_cloud, options_.global_localization_min_score(), + &score, &pose_estimate, &covariance)) { + trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); + } else { + return; + } + } else { + if (!submap_scan_matcher->fast_correlative_scan_matcher->Match( + initial_pose, filtered_point_cloud, options_.min_score(), &score, + &pose_estimate, &covariance)) { + return; + } + // We've reported a successful local match. + CHECK_GT(score, options_.min_score()); + // 'covariance' is unchanged as (submap <- map) is a translation. + if (covariance.trace() > options_.max_covariance_trace()) { + return; + } + + { + common::MutexLocker locker(&mutex_); + score_histogram_.Add(score); + } + } + + // Use the CSM estimate as both the initial and previous pose. This has the + // effect that, in the absence of better information, we prefer the original + // CSM estimate. We also prefer to use the CSM covariance estimate for loop + // closure constraints since it is representative of a larger area (as opposed + // to the local Ceres estimate of covariance). + ceres::Solver::Summary unused_summary; + kalman_filter::Pose2DCovariance unused_covariance; + ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud, + *submap_scan_matcher->probability_grid, + &pose_estimate, &unused_covariance, + &unused_summary); + + const transform::Rigid2d constraint_transform = + ComputeSubmapPose(*submap).inverse() * pose_estimate; + constraint->reset(new OptimizationProblem::Constraint{ + submap_index, + scan_index, + {constraint_transform, ComputeSqrtLambda(covariance)}, + OptimizationProblem::Constraint::INTER_SUBMAP}); + + if (options_.log_matches()) { + const transform::Rigid2d difference = + initial_pose.inverse() * pose_estimate; + std::ostringstream info; + info << "Scan index " << scan_index << " with " + << filtered_point_cloud.size() << " points on submap " << submap_index + << " differs by translation " << std::fixed << std::setprecision(2) + << difference.translation().norm() << " rotation " + << std::setprecision(3) << std::abs(difference.normalized_angle()) + << " with score " << std::setprecision(1) << 100. * score + << "% covariance trace " << std::scientific << std::setprecision(4) + << covariance.trace() << "."; + LOG(INFO) << info.str(); + } +} + +void ConstraintBuilder::FinishComputation(const int computation_index) { + Result result; + std::unique_ptr> callback; + { + common::MutexLocker locker(&mutex_); + if (--pending_computations_[computation_index] == 0) { + pending_computations_.erase(computation_index); + } + if (pending_computations_.empty()) { + CHECK_EQ(submap_queued_work_items_.size(), 0); + if (when_done_ != nullptr) { + for (const std::unique_ptr& + constraint : constraints_) { + if (constraint != nullptr) { + result.push_back(*constraint); + } + } + if (options_.log_matches()) { + LOG(INFO) << constraints_.size() << " computations resulted in " + << result.size() << " additional constraints."; + LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10); + } + constraints_.clear(); + callback = std::move(when_done_); + when_done_.reset(); + } + } + } + if (callback != nullptr) { + (*callback)(result); + } +} + +int ConstraintBuilder::GetNumFinishedScans() { + common::MutexLocker locker(&mutex_); + if (pending_computations_.empty()) { + return current_computation_; + } + return pending_computations_.begin()->first; +} + +} // namespace sparse_pose_graph +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h new file mode 100644 index 0000000..1cd94ef --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.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_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ + +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/histogram.h" +#include "cartographer/common/math.h" +#include "cartographer/common/mutex.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" +#include "cartographer/mapping/trajectory_connectivity.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_2d/sparse_pose_graph/optimization_problem.h" +#include "cartographer/mapping_2d/submaps.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/point_cloud.h" +#include "cartographer/sensor/voxel_filter.h" + +namespace cartographer { +namespace mapping_2d { +namespace sparse_pose_graph { + +// Returns (map <- submap) where 'submap' is a coordinate system at the origin +// of the Submap. +transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap); + +// Asynchronously computes constraints. +// +// Intermingle an arbitrary number of calls to MaybeAddConstraint() or +// MaybeAddGlobalConstraint, then call WhenDone(). After all computations are +// done the 'callback' will be called with the result and another +// MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. +// +// This class is thread-safe. +class ConstraintBuilder { + public: + using Constraint = mapping::SparsePoseGraph::Constraint2D; + using Result = std::vector; + + ConstraintBuilder( + const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& + options, + common::ThreadPool* thread_pool); + ~ConstraintBuilder(); + + ConstraintBuilder(const ConstraintBuilder&) = delete; + ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_index', and the 'point_cloud' for 'scan_index'. + // The 'initial_pose' is relative to the 'submap'. + // + // The pointees of 'submap' and 'point_cloud' must stay valid until all + // computations are finished. + void MaybeAddConstraint(int submap_index, const mapping::Submap* submap, + int scan_index, + const sensor::PointCloud2D* point_cloud, + const transform::Rigid2d& initial_relative_pose); + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_index' and the 'point_cloud' for 'scan_index'. This performs + // full-submap matching. + // + // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and + // the 'submap' should be from 'submap_trajectory'. The + // 'trajectory_connectivity' is updated if the full-submap match succeeds. + // + // The pointees of 'submap' and 'point_cloud' must stay valid until all + // computations are finished. + void MaybeAddGlobalConstraint( + int submap_index, const mapping::Submap* submap, int scan_index, + const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const sensor::PointCloud2D* point_cloud); + + // Must be called after all computations related to 'scan_index' are added. + void NotifyEndOfScan(const int scan_index); + + // Registers the 'callback' to be called with the results, after all + // computations triggered by MaybeAddConstraint() have finished. + void WhenDone(std::function callback); + + // Computes the inverse square root of 'covariance'. + Eigen::Matrix3d ComputeSqrtLambda(const Eigen::Matrix3d& covariance) const; + + // Returns the number of consecutive finished scans. + int GetNumFinishedScans(); + + private: + struct SubmapScanMatcher { + const ProbabilityGrid* probability_grid; + std::unique_ptr + fast_correlative_scan_matcher; + }; + + // Either schedules the 'work_item', or if needed, schedules the scan matcher + // construction and queues the 'work_item'. + void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + int submap_index, const ProbabilityGrid* submap, + std::function work_item) REQUIRES(mutex_); + + // Constructs the scan matcher for a 'submap', then schedules its work items. + void ConstructSubmapScanMatcher(int submap_index, + const ProbabilityGrid* submap) + EXCLUDES(mutex_); + + // Returns the scan matcher for a submap, which has to exist. + const SubmapScanMatcher* GetSubmapScanMatcher(int submap_index) + EXCLUDES(mutex_); + + // Runs in a background thread and does computations for an additional + // constraint, assuming 'submap' and 'point_cloud' do not change anymore. + // If 'match_full_submap' is true, and global localization succeeds, will + // connect 'scan_trajectory' and 'submap_trajectory' in + // 'trajectory_connectivity'. + // As output, it may create a new Constraint in 'constraint'. + void ComputeConstraint( + int submap_index, const mapping::Submap* submap, int scan_index, + const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, bool match_full_submap, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const sensor::PointCloud2D* point_cloud, + const transform::Rigid2d& initial_relative_pose, + std::unique_ptr* constraint) EXCLUDES(mutex_); + + // Decrements the 'pending_computations_' count. If all computations are done, + // runs the 'when_done_' callback and resets the state. + void FinishComputation(int computation_index) EXCLUDES(mutex_); + + const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_; + common::ThreadPool* thread_pool_; + common::Mutex mutex_; + + // 'callback' set by WhenDone(). + std::unique_ptr> when_done_ + GUARDED_BY(mutex_); + + // Index of the scan in reaction to which computations are currently + // added. This is always the highest scan index seen so far, even when older + // scans are matched against a new submap. + int current_computation_ GUARDED_BY(mutex_) = 0; + + // For each added scan, maps to the number of pending computations that were + // added for it. + std::map pending_computations_ GUARDED_BY(mutex_); + + // Constraints currently being computed in the background. A deque is used to + // keep pointers valid when adding more entries. + std::deque> constraints_ GUARDED_BY(mutex_); + + // Map of already constructed scan matchers by 'submap_index'. + std::map submap_scan_matchers_ GUARDED_BY(mutex_); + + // Map by 'submap_index' of scan matchers under construction, and the work + // to do once construction is done. + std::map>> submap_queued_work_items_ + GUARDED_BY(mutex_); + + common::FixedRatioSampler sampler_; + const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_; + scan_matching::CeresScanMatcher ceres_scan_matcher_; + + // Histogram of scan matcher scores. + common::Histogram score_histogram_ GUARDED_BY(mutex_); +}; + +} // namespace sparse_pose_graph +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc new file mode 100644 index 0000000..69ed7ec --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -0,0 +1,207 @@ +/* + * 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/sparse_pose_graph/optimization_problem.h" + +#include +#include +#include +#include +#include +#include + +#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/histogram.h" +#include "cartographer/common/math.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { +namespace sparse_pose_graph { + +namespace { + +// Converts a pose into the 3 optimization variable format used for Ceres: +// translation in x and y, followed by the rotation angle representing the +// orientation. +std::array FromPose(const transform::Rigid2d& pose) { + return {{pose.translation().x(), pose.translation().y(), + pose.normalized_angle()}}; +} + +// Converts a pose as represented for Ceres back to an transform::Rigid2d pose. +transform::Rigid2d ToPose(const std::array& values) { + return transform::Rigid2d({values[0], values[1]}, values[2]); +} + +} // namespace + +OptimizationProblem::OptimizationProblem( + const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& + options) + : options_(options) {} + +OptimizationProblem::~OptimizationProblem() {} + +void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { + options_.mutable_ceres_solver_options()->set_max_num_iterations( + max_num_iterations); +} + +void OptimizationProblem::Solve( + const std::vector& constraints, + const std::vector& trajectories, + const std::vector& initial_point_cloud_poses, + std::vector* point_cloud_poses, + std::vector* submap_transforms) { + if (point_cloud_poses->empty()) { + // Nothing to optimize. + return; + } + + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + + // Set the starting point. + std::vector> C_submaps(submap_transforms->size()); + std::vector> C_point_clouds(point_cloud_poses->size()); + for (size_t i = 0; i != submap_transforms->size(); ++i) { + C_submaps[i] = FromPose((*submap_transforms)[i]); + problem.AddParameterBlock(C_submaps[i].data(), 3); + } + for (size_t j = 0; j != point_cloud_poses->size(); ++j) { + C_point_clouds[j] = FromPose((*point_cloud_poses)[j]); + problem.AddParameterBlock(C_point_clouds[j].data(), 3); + } + + // Fix the pose of the first submap. + problem.SetParameterBlockConstant(C_submaps[0].data()); + + // Add cost functions for intra- and inter-submap constraints. + std::vector> + constraints_residual_blocks; + for (const Constraint& constraint : constraints) { + CHECK_GE(constraint.i, 0); + CHECK_LT(constraint.i, submap_transforms->size()); + CHECK_GE(constraint.j, 0); + CHECK_LT(constraint.j, point_cloud_poses->size()); + constraints_residual_blocks.emplace_back( + constraint.tag, + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new SpaCostFunction(constraint.pose)), + // Only loop closure constraints should have a loss function. + constraint.tag == Constraint::INTER_SUBMAP + ? new ceres::HuberLoss(options_.huber_scale()) + : nullptr, + C_submaps[constraint.i].data(), + C_point_clouds[constraint.j].data())); + } + + // Add penalties for changes between consecutive scans. + CHECK(!point_cloud_poses->empty()); + const Eigen::DiagonalMatrix consecutive_pose_change_penalty_matrix( + options_.consecutive_scan_translation_penalty_factor(), + options_.consecutive_scan_translation_penalty_factor(), + options_.consecutive_scan_rotation_penalty_factor()); + CHECK_GE(initial_point_cloud_poses.size(), point_cloud_poses->size()); + CHECK_GE(trajectories.size(), point_cloud_poses->size()); + + // The poses in initial_point_cloud_poses and point_cloud_poses are + // interleaved from multiple trajectories (although the points from a given + // trajectory are in time order). 'last_pose_indices[trajectory]' is the index + // into 'initial_point_cloud_poses' of the most-recent pose on 'trajectory'. + std::map last_pose_indices; + + for (size_t j = 0; j != point_cloud_poses->size(); ++j) { + const mapping::Submaps* trajectory = trajectories[j]; + // This pose has a predecessor. + if (last_pose_indices.count(trajectory) != 0) { + const int last_pose_index = last_pose_indices[trajectory]; + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new SpaCostFunction(Constraint::Pose{ + initial_point_cloud_poses[last_pose_index].inverse() * + initial_point_cloud_poses[j], + consecutive_pose_change_penalty_matrix})), + nullptr /* loss function */, C_point_clouds[last_pose_index].data(), + C_point_clouds[j].data()); + } + last_pose_indices[trajectory] = j; + } + + // Solve. + ceres::Solver::Summary summary; + ceres::Solve( + common::CreateCeresSolverOptions(options_.ceres_solver_options()), + &problem, &summary); + + if (options_.log_residual_histograms()) { + common::Histogram intra_submap_xy_residuals; + common::Histogram intra_submap_theta_residuals; + common::Histogram inter_submap_xy_residuals; + common::Histogram inter_submap_theta_residuals; + for (auto constraint_residual_block : constraints_residual_blocks) { + ceres::Problem::EvaluateOptions options; + options.apply_loss_function = false; + options.residual_blocks = {constraint_residual_block.second}; + std::vector residuals; + problem.Evaluate(options, nullptr, &residuals, nullptr, nullptr); + CHECK_EQ(3, residuals.size()); + switch (constraint_residual_block.first) { + case Constraint::INTRA_SUBMAP: + intra_submap_xy_residuals.Add(common::Pow2(residuals[0]) + + common::Pow2(residuals[1])); + intra_submap_theta_residuals.Add(common::Pow2(residuals[2])); + break; + case Constraint::INTER_SUBMAP: + inter_submap_xy_residuals.Add(common::Pow2(residuals[0]) + + common::Pow2(residuals[1])); + inter_submap_theta_residuals.Add(common::Pow2(residuals[2])); + break; + } + } + LOG(INFO) << "Intra-submap x^2 + y^2 residual histogram:\n" + << intra_submap_xy_residuals.ToString(10); + LOG(INFO) << "Intra-submap theta^2 residual histogram:\n" + << intra_submap_theta_residuals.ToString(10); + LOG(INFO) << "Inter-submap x^2 + y^2 residual histogram:\n" + << inter_submap_xy_residuals.ToString(10); + LOG(INFO) << "Inter-submap theta^2 residual histogram:\n" + << inter_submap_theta_residuals.ToString(10); + } + ceres::Solve( + common::CreateCeresSolverOptions(options_.ceres_solver_options()), + &problem, &summary); + + if (options_.log_solver_summary()) { + LOG(INFO) << summary.FullReport(); + } + + // Store the result. + for (size_t i = 0; i != submap_transforms->size(); ++i) { + (*submap_transforms)[i] = ToPose(C_submaps[i]); + } + for (size_t j = 0; j != point_cloud_poses->size(); ++j) { + (*point_cloud_poses)[j] = ToPose(C_point_clouds[j]); + } +} + +} // namespace sparse_pose_graph +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h new file mode 100644 index 0000000..e58bf03 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -0,0 +1,126 @@ +/* + * 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_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ + +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" +#include "cartographer/mapping_2d/submaps.h" + +namespace cartographer { +namespace mapping_2d { +namespace sparse_pose_graph { + +// Implements the SPA loop closure method. +class OptimizationProblem { + public: + using Constraint = mapping::SparsePoseGraph::Constraint2D; + + class SpaCostFunction { + public: + explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} + + // Computes the error in the proposed change to the scan-to-submap alignment + // 'zbar_ij' where submap pose 'c_i' and scan pose 'c_j' are both in an + // arbitrary common frame. + template + static std::array ComputeUnscaledError( + const transform::Rigid2d& zbar_ij, const T* const c_i, + const T* const c_j) { + const T cos_theta_i = cos(c_i[2]); + const T sin_theta_i = sin(c_i[2]); + const T delta_x = c_j[0] - c_i[0]; + const T delta_y = c_j[1] - c_i[1]; + const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, + -sin_theta_i * delta_x + cos_theta_i * delta_y, + c_j[2] - c_i[2]}; + return {{T(zbar_ij.translation().x()) - h[0], + T(zbar_ij.translation().y()) - h[1], + common::NormalizeAngleDifference(T(zbar_ij.rotation().angle()) - + h[2])}}; + } + + // Scales the result of ComputeUnscaledError scaled by 'sqrt_Lambda_ij' and + // stores it in 'e'. + template + static void ComputeScaledError(const Constraint::Pose& pose, + const T* const c_i, const T* const c_j, + T* const e) { + std::array e_ij = ComputeUnscaledError(pose.zbar_ij, c_i, c_j); + (Eigen::Map>(e)) = + pose.sqrt_Lambda_ij.cast() * + Eigen::Map>(e_ij.data()); + } + + template + bool operator()(const T* const c_i, const T* const c_j, T* e) const { + ComputeScaledError(pose_, c_i, c_j, e); + return true; + } + + private: + const Constraint::Pose pose_; + }; + + explicit OptimizationProblem( + const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& + options); + ~OptimizationProblem(); + + OptimizationProblem(const OptimizationProblem&) = delete; + OptimizationProblem& operator=(const OptimizationProblem&) = delete; + + void SetMaxNumIterations(int32 max_num_iterations); + + // Computes the optimized poses. The point cloud at 'point_cloud_poses[i]' + // belongs to 'trajectories[i]'. Within a given trajectory, scans are expected + // to be contiguous. + void Solve(const std::vector& constraints, + const std::vector& trajectories, + const std::vector& initial_point_cloud_poses, + std::vector* point_cloud_poses, + std::vector* submap_transforms); + + private: + class TieToPoseCostFunction { + public: + explicit TieToPoseCostFunction(const Constraint::Pose& pose) + : pose_(pose) {} + + template + bool operator()(const T* const c_j, T* e) const; + + private: + const Constraint::Pose pose_; + }; + + mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; +}; + +} // namespace sparse_pose_graph +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ diff --git a/cartographer/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/cartographer/mapping_2d/sparse_pose_graph_test.cc new file mode 100644 index 0000000..78bea0d --- /dev/null +++ b/cartographer/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -0,0 +1,271 @@ +/* + * 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/sparse_pose_graph.h" + +#include +#include +#include + +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_2d { +namespace { + +class SparsePoseGraphTest : public ::testing::Test { + protected: + SparsePoseGraphTest() : thread_pool_(1) { + // Builds a wavy, irregularly circular point cloud that is unique + // rotationally. This gives us good rotational texture and avoids any + // possibility of the CeresScanMatcher preferring free space (> + // kMinProbability) to unknown space (== kMinProbability). + for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) { + const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f); + point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t)); + } + + { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + resolution = 0.05, + half_length = 21., + num_laser_fans = 1, + output_debug_images = false, + laser_fan_inserter = { + insert_free_space = true, + hit_probability = 0.53, + miss_probability = 0.495, + }, + })text"); + submaps_ = common::make_unique( + CreateSubmapsOptions(parameter_dictionary.get())); + } + + { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + optimize_every_n_scans = 1000, + also_match_to_new_submaps = true, + constraint_builder = { + sampling_ratio = 1., + max_constraint_distance = 6., + adaptive_voxel_filter = { + max_length = 1e-5, + min_num_points = 1, + max_range = 50., + }, + min_score = 0.5, + global_localization_min_score = 0.6, + max_covariance_trace = 10., + lower_covariance_eigenvalue_bound = 1e-6, + log_matches = true, + fast_correlative_scan_matcher = { + linear_search_window = 3., + angular_search_window = 0.1, + branch_and_bound_depth = 3, + covariance_scale = 1., + }, + ceres_scan_matcher = { + occupied_space_cost_functor_weight = 20., + previous_pose_translation_delta_cost_functor_weight = 10., + initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + covariance_scale = 1., + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 50, + num_threads = 1, + }, + }, + fast_correlative_scan_matcher_3d = { + branch_and_bound_depth = 3, + full_resolution_depth = 3, + rotational_histogram_size = 30, + min_rotational_score = 0.1, + linear_xy_search_window = 4., + linear_z_search_window = 4., + angular_search_window = 0.1, + }, + ceres_scan_matcher_3d = { + occupied_space_cost_functor_weight_0 = 20., + previous_pose_translation_delta_cost_functor_weight = 10., + initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + covariance_scale = 1., + only_optimize_yaw = true, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 50, + num_threads = 1, + }, + }, + }, + optimization_problem = { + acceleration_scale = 1., + rotation_scale = 1e2, + huber_scale = 1., + consecutive_scan_translation_penalty_factor = 0., + consecutive_scan_rotation_penalty_factor = 0., + log_solver_summary = true, + log_residual_histograms = true, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 200, + num_threads = 1, + }, + }, + max_num_final_iterations = 200, + global_sampling_ratio = 0.01, + })text"); + sparse_pose_graph_ = common::make_unique( + mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), + &thread_pool_, &constant_node_data_); + } + + current_pose_ = transform::Rigid2d::Identity(); + } + + void MoveRelativeWithNoise(const transform::Rigid2d& movement, + const transform::Rigid2d& noise) { + current_pose_ = current_pose_ * movement; + const sensor::PointCloud2D new_point_cloud = sensor::TransformPointCloud2D( + point_cloud_, current_pose_.inverse().cast()); + kalman_filter::Pose2DCovariance covariance = + kalman_filter::Pose2DCovariance::Identity(); + const mapping::Submap* const matching_submap = + submaps_->Get(submaps_->matching_index()); + std::vector insertion_submaps; + for (int insertion_index : submaps_->insertion_indices()) { + insertion_submaps.push_back(submaps_->Get(insertion_index)); + } + const sensor::LaserFan laser_fan{ + Eigen::Vector2f::Zero(), new_point_cloud, {}}; + const transform::Rigid2d pose_estimate = noise * current_pose_; + submaps_->InsertLaserFan( + TransformLaserFan(laser_fan, pose_estimate.cast())); + sparse_pose_graph_->AddScan(common::FromUniversal(0), + transform::Rigid3d::Identity(), laser_fan, + pose_estimate, covariance, submaps_.get(), + matching_submap, insertion_submaps); + } + + void MoveRelative(const transform::Rigid2d& movement) { + MoveRelativeWithNoise(movement, transform::Rigid2d::Identity()); + } + + sensor::PointCloud2D point_cloud_; + std::unique_ptr submaps_; + std::deque constant_node_data_; + common::ThreadPool thread_pool_; + std::unique_ptr sparse_pose_graph_; + transform::Rigid2d current_pose_; +}; + +TEST_F(SparsePoseGraphTest, EmptyMap) { + sparse_pose_graph_->RunFinalOptimization(); + const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + EXPECT_THAT(nodes.size(), ::testing::Eq(0)); +} + +TEST_F(SparsePoseGraphTest, NoMovement) { + MoveRelative(transform::Rigid2d::Identity()); + MoveRelative(transform::Rigid2d::Identity()); + MoveRelative(transform::Rigid2d::Identity()); + sparse_pose_graph_->RunFinalOptimization(); + const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + EXPECT_THAT(nodes.size(), ::testing::Eq(3)); + EXPECT_THAT(nodes[0].pose, + transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); + EXPECT_THAT(nodes[1].pose, + transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); + EXPECT_THAT(nodes[2].pose, + transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); +} + +TEST_F(SparsePoseGraphTest, NoOverlappingScans) { + std::mt19937 rng(0); + std::uniform_real_distribution distribution(-1., 1.); + std::vector poses; + for (int i = 0; i != 4; ++i) { + MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.)); + poses.emplace_back(current_pose_); + } + sparse_pose_graph_->RunFinalOptimization(); + const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + for (int i = 0; i != 4; ++i) { + EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2)) + << i; + } +} + +TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) { + std::mt19937 rng(0); + std::uniform_real_distribution distribution(-1., 1.); + std::vector poses; + for (int i = 0; i != 5; ++i) { + MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.)); + poses.emplace_back(current_pose_); + } + sparse_pose_graph_->RunFinalOptimization(); + const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + for (int i = 0; i != 5; ++i) { + EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2)) + << i; + } +} + +TEST_F(SparsePoseGraphTest, OverlappingScans) { + std::mt19937 rng(0); + std::uniform_real_distribution distribution(-1., 1.); + std::vector ground_truth; + std::vector poses; + for (int i = 0; i != 5; ++i) { + const double noise_x = 0.1 * distribution(rng); + const double noise_y = 0.1 * distribution(rng); + const double noise_orientation = 0.1 * distribution(rng); + transform::Rigid2d noise({noise_x, noise_y}, noise_orientation); + MoveRelativeWithNoise( + transform::Rigid2d({0.15 * distribution(rng), 0.4}, 0.), noise); + ground_truth.emplace_back(current_pose_); + poses.emplace_back(noise * current_pose_); + } + sparse_pose_graph_->RunFinalOptimization(); + const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + transform::Rigid2d true_movement = + ground_truth.front().inverse() * ground_truth.back(); + transform::Rigid2d movement_before = poses.front().inverse() * poses.back(); + transform::Rigid2d error_before = movement_before.inverse() * true_movement; + transform::Rigid3d optimized_movement = + nodes.front().pose.inverse() * nodes.back().pose; + transform::Rigid2d optimized_error = + transform::Project2D(optimized_movement).inverse() * true_movement; + EXPECT_THAT(std::abs(optimized_error.normalized_angle()), + ::testing::Lt(std::abs(error_before.normalized_angle()))); + EXPECT_THAT(optimized_error.translation().norm(), + ::testing::Lt(error_before.translation().norm())); +} + +} // namespace +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/submaps.cc b/cartographer/cartographer/mapping_2d/submaps.cc new file mode 100644 index 0000000..d9aa067 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/submaps.cc @@ -0,0 +1,180 @@ +/* + * 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/submaps.h" + +#include +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/make_unique.h" +#include "glog/logging.h" +#include "webp/encode.h" + +namespace cartographer { +namespace mapping_2d { + +namespace { + +void WriteDebugImage(const string& filename, + const ProbabilityGrid& probability_grid) { + constexpr int kUnknown = 128; + const mapping_2d::CellLimits& cell_limits = + probability_grid.limits().cell_limits(); + const int width = cell_limits.num_x_cells; + const int height = cell_limits.num_y_cells; + std::vector rgb; + for (const Eigen::Array2i& xy_index : mapping_2d::XYIndexRangeIterator( + probability_grid.limits().cell_limits())) { + CHECK(probability_grid.limits().Contains(xy_index)); + const uint8_t value = + probability_grid.IsKnown(xy_index) + ? common::RoundToInt( + (1. - probability_grid.GetProbability(xy_index)) * 255 + 0) + : kUnknown; + rgb.push_back(value); + rgb.push_back(value); + rgb.push_back(value); + } + uint8_t* output = nullptr; + size_t output_size = + WebPEncodeLosslessRGB(rgb.data(), width, height, 3 * width, &output); + std::unique_ptr output_deleter(output, std::free); + std::ofstream output_file(filename, std::ios::out | std::ios::binary); + output_file.write(reinterpret_cast(output), output_size); + output_file.close(); + CHECK(output_file) << "Writing " << filename << " failed."; +} + +} // namespace + +ProbabilityGrid ComputeCroppedProbabilityGrid( + const ProbabilityGrid& probability_grid) { + Eigen::Array2i offset; + CellLimits limits; + probability_grid.ComputeCroppedLimits(&offset, &limits); + const double resolution = probability_grid.limits().resolution(); + const double max_x = probability_grid.limits().centered_limits().max().x() - + resolution * offset.y(); + const double max_y = probability_grid.limits().centered_limits().max().y() - + resolution * offset.x(); + ProbabilityGrid cropped_grid(MapLimits(resolution, max_x, max_y, limits)); + cropped_grid.StartUpdate(); + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { + if (probability_grid.IsKnown(xy_index + offset)) { + cropped_grid.SetProbability( + xy_index, probability_grid.GetProbability(xy_index + offset)); + } + } + return cropped_grid; +} + +proto::SubmapsOptions CreateSubmapsOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::SubmapsOptions options; + options.set_resolution(parameter_dictionary->GetDouble("resolution")); + options.set_half_length(parameter_dictionary->GetDouble("half_length")); + options.set_num_laser_fans( + parameter_dictionary->GetNonNegativeInt("num_laser_fans")); + options.set_output_debug_images( + parameter_dictionary->GetBool("output_debug_images")); + *options.mutable_laser_fan_inserter_options() = CreateLaserFanInserterOptions( + parameter_dictionary->GetDictionary("laser_fan_inserter").get()); + CHECK_GT(options.num_laser_fans(), 0); + return options; +} + +Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin, + const int begin_laser_fan_index) + : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.), + begin_laser_fan_index), + probability_grid(limits) {} + +Submaps::Submaps(const proto::SubmapsOptions& options) + : options_(options), + laser_fan_inserter_(options.laser_fan_inserter_options()) { + // We always want to have at least one likelihood field which we can return, + // and will create it at the origin in absence of a better choice. + AddSubmap(Eigen::Vector2f::Zero()); +} + +void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { + CHECK_LT(num_laser_fans_, std::numeric_limits::max()); + ++num_laser_fans_; + for (const int index : insertion_indices()) { + Submap* submap = submaps_[index].get(); + CHECK(submap->finished_probability_grid == nullptr); + laser_fan_inserter_.Insert(laser_fan, &submap->probability_grid); + submap->end_laser_fan_index = num_laser_fans_; + } + ++num_laser_fans_in_last_submap_; + if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) { + AddSubmap(laser_fan.origin); + } +} + +const Submap* Submaps::Get(int index) const { + CHECK_GE(index, 0); + CHECK_LT(index, size()); + return submaps_[index].get(); +} + +int Submaps::size() const { return submaps_.size(); } + +void Submaps::SubmapToProto( + const int index, const std::vector&, + const transform::Rigid3d&, + mapping::proto::SubmapQuery::Response* const response) { + AddProbabilityGridToResponse(Get(index)->local_pose(), + Get(index)->probability_grid, response); +} + +void Submaps::FinishSubmap(int index) { + // Crop the finished Submap before inserting a new Submap to reduce peak + // memory usage a bit. + Submap* submap = submaps_[index].get(); + CHECK(submap->finished_probability_grid == nullptr); + submap->probability_grid = + ComputeCroppedProbabilityGrid(submap->probability_grid); + submap->finished_probability_grid = &submap->probability_grid; + if (options_.output_debug_images()) { + // Output the Submap that won't be changed from now on. + WriteDebugImage("submap" + std::to_string(index) + ".webp", + submap->probability_grid); + } +} + +void Submaps::AddSubmap(const Eigen::Vector2f& origin) { + if (size() > 1) { + FinishSubmap(size() - 2); + } + submaps_.push_back(common::make_unique( + MapLimits(options_.resolution(), + Eigen::AlignedBox2d( + origin.cast() - + options_.half_length() * Eigen::Vector2d::Ones(), + origin.cast() + + options_.half_length() * Eigen::Vector2d::Ones())), + origin, num_laser_fans_)); + LOG(INFO) << "Added submap " << size(); + num_laser_fans_in_last_submap_ = 0; +} + +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/submaps.h b/cartographer/cartographer/mapping_2d/submaps.h new file mode 100644 index 0000000..d035ea9 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/submaps.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ +#define CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/proto/submaps.pb.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/map_limits.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/proto/submaps_options.pb.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping_2d { + +ProbabilityGrid ComputeCroppedProbabilityGrid( + const ProbabilityGrid& probability_grid); + +proto::SubmapsOptions CreateSubmapsOptions( + common::LuaParameterDictionary* parameter_dictionary); + +struct Submap : public mapping::Submap { + Submap(const MapLimits& limits, const Eigen::Vector2f& origin, + int begin_laser_fan_index); + + ProbabilityGrid probability_grid; +}; + +// A container of Submaps. +class Submaps : public mapping::Submaps { + public: + explicit Submaps(const proto::SubmapsOptions& options); + + Submaps(const Submaps&) = delete; + Submaps& operator=(const Submaps&) = delete; + + const Submap* Get(int index) const override; + int size() const override; + void SubmapToProto( + int index, const std::vector& trajectory_nodes, + const transform::Rigid3d& global_submap_pose, + mapping::proto::SubmapQuery::Response* response) override; + + // Inserts 'laser_fan' into the Submap collection. + void InsertLaserFan(const sensor::LaserFan& laser_fan); + + private: + void FinishSubmap(int index); + void AddSubmap(const Eigen::Vector2f& origin); + + const proto::SubmapsOptions options_; + + std::vector> submaps_; + LaserFanInserter laser_fan_inserter_; + + // Number of LaserFans inserted. + int num_laser_fans_ = 0; + + // Number of LaserFans inserted since the last Submap was added. + int num_laser_fans_in_last_submap_ = 0; +}; + +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ diff --git a/cartographer/cartographer/mapping_2d/submaps_test.cc b/cartographer/cartographer/mapping_2d/submaps_test.cc new file mode 100644 index 0000000..302cfff --- /dev/null +++ b/cartographer/cartographer/mapping_2d/submaps_test.cc @@ -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. + */ + +#include "cartographer/mapping_2d/submaps.h" + +#include +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/port.h" +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_2d { +namespace { + +TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { + constexpr int kNumLaserFans = 10; + auto parameter_dictionary = common::MakeDictionary( + "return {" + "resolution = 0.05, " + "half_length = 10., " + "num_laser_fans = " + + std::to_string(kNumLaserFans) + + ", " + "output_debug_images = false, " + "laser_fan_inserter = {" + "insert_free_space = true, " + "hit_probability = 0.53, " + "miss_probability = 0.495, " + "}," + "}"); + Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; + auto num_inserted = [&submaps](const int i) { + return submaps.Get(i)->end_laser_fan_index - + submaps.Get(i)->begin_laser_fan_index; + }; + for (int i = 0; i != 1000; ++i) { + submaps.InsertLaserFan({Eigen::Vector2f::Zero(), {}, {}}); + const int matching = submaps.matching_index(); + // Except for the first, maps should only be returned after enough scans. + if (matching != 0) { + EXPECT_LE(kNumLaserFans, num_inserted(matching)); + } + } + for (int i = 0; i != submaps.size() - 2; ++i) { + // Submaps should not be left without the right number of scans in them. + EXPECT_EQ(kNumLaserFans * 2, num_inserted(i)); + EXPECT_EQ(i * kNumLaserFans, submaps.Get(i)->begin_laser_fan_index); + EXPECT_EQ((i + 2) * kNumLaserFans, submaps.Get(i)->end_laser_fan_index); + } +} + +} // namespace +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_2d/xy_index.h b/cartographer/cartographer/mapping_2d/xy_index.h new file mode 100644 index 0000000..4806fa4 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/xy_index.h @@ -0,0 +1,148 @@ +/* + * 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_XY_INDEX_H_ +#define CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ + +#include +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_2d { + +struct CellLimits { + CellLimits() = default; + CellLimits(int init_num_x_cells, int init_num_y_cells) + : num_x_cells(init_num_x_cells), num_y_cells(init_num_y_cells) {} + + int num_x_cells = 0; + int num_y_cells = 0; +}; + +// Iterates in row-major order through a range of xy-indices. +class XYIndexRangeIterator + : public std::iterator { + public: + // Constructs a new iterator for the specified range. The range is cropped to + // fit within the given 'cell_limits'. + XYIndexRangeIterator(const CellLimits& cell_limits, + const Eigen::Array2i& min_xy_index, + const Eigen::Array2i& max_xy_index) + : XYIndexRangeIterator( + min_xy_index.max(Eigen::Array2i::Zero()), + max_xy_index.min(Eigen::Array2i(cell_limits.num_x_cells - 1, + cell_limits.num_y_cells - 1))) {} + + // Constructs a new iterator for everything contained in 'cell_limits'. + explicit XYIndexRangeIterator(const CellLimits& cell_limits) + : XYIndexRangeIterator(Eigen::Array2i::Zero(), + Eigen::Array2i(cell_limits.num_x_cells - 1, + cell_limits.num_y_cells - 1)) {} + + XYIndexRangeIterator(const XYIndexRangeIterator& other) + : min_xy_index_(other.min_xy_index_), + max_xy_index_(other.max_xy_index_), + xy_index_(other.xy_index_) {} + + XYIndexRangeIterator& operator=(const XYIndexRangeIterator& other) { + min_xy_index_ = other.min_xy_index_; + max_xy_index_ = other.max_xy_index_; + xy_index_ = other.xy_index_; + return *this; + } + + XYIndexRangeIterator& operator++() { + // This is a necessary evil. Bounds checking is very expensive and needs to + // be avoided in production. We have unit tests that exercise this check + // in debug mode. + DCHECK(*this != end()); + if (xy_index_.x() < max_xy_index_.x()) { + ++xy_index_.x(); + } else { + xy_index_.x() = min_xy_index_.x(); + ++xy_index_.y(); + } + return *this; + } + + Eigen::Array2i& operator*() { return xy_index_; } + + bool operator==(const XYIndexRangeIterator& other) const { + return (xy_index_ == other.xy_index_).all(); + } + + bool operator!=(const XYIndexRangeIterator& other) const { + return !operator==(other); + } + + XYIndexRangeIterator begin() { + return XYIndexRangeIterator(min_xy_index_, max_xy_index_); + } + + XYIndexRangeIterator end() { + XYIndexRangeIterator it = begin(); + it.xy_index_ = Eigen::Array2i(min_xy_index_.x(), max_xy_index_.y() + 1); + return it; + } + + private: + XYIndexRangeIterator(const Eigen::Array2i& min_xy_index, + const Eigen::Array2i& max_xy_index) + : min_xy_index_(min_xy_index), + max_xy_index_(max_xy_index), + xy_index_(min_xy_index) {} + + Eigen::Array2i min_xy_index_; + Eigen::Array2i max_xy_index_; + Eigen::Array2i xy_index_; +}; + +// Returns the center of the resolution interval containing x. +inline double Center(double x, double resolution) { + return (std::floor(x / resolution) + 0.5) * resolution; +} + +// Returns the index of the cell containing the point (x, y) which is allowed to +// lie outside the map. +inline Eigen::Array2i GetXYIndexOfCellContainingPoint(double x, double y, + double centered_max_x, + double centered_max_y, + double resolution) { + // Index values are row major and the top left has Eigen::Array2i::Zero() and + // contains (centered_max_x, centered_max_y). We need to flip and rotate. + const int cell_x = common::RoundToInt((centered_max_y - y) / resolution); + const int cell_y = common::RoundToInt((centered_max_x - x) / resolution); + return Eigen::Array2i(cell_x, cell_y); +} + +// Returns the index into the vector with 'stride' for the cell with 'xy_index'. +inline int GetIndexOfCell(const Eigen::Array2i& xy_index, int stride) { + CHECK_GE(xy_index.x(), 0); + CHECK_GE(xy_index.y(), 0); + return stride * xy_index.y() + xy_index.x(); +} + +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ diff --git a/cartographer/cartographer/mapping_2d/xy_index_test.cc b/cartographer/cartographer/mapping_2d/xy_index_test.cc new file mode 100644 index 0000000..8193530 --- /dev/null +++ b/cartographer/cartographer/mapping_2d/xy_index_test.cc @@ -0,0 +1,46 @@ +/* + * 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/xy_index.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_2d { +namespace { + +TEST(XYIndexTest, XYIndexRangeIterator) { + const CellLimits limits(5, 5); + const Eigen::Array2i min(1, 2); + const Eigen::Array2i max(3, 4); + XYIndexRangeIterator it(limits, min, max); + EXPECT_TRUE((min == *it.begin()).all()) << *it.begin(); + EXPECT_TRUE((Eigen::Array2i(1, 5) == *it.end()).all()) << *it.end(); + EXPECT_TRUE((min == *it).all()) << *it; + int num_indices = 0; + for (const Eigen::Array2i& xy_index : + XYIndexRangeIterator(limits, min, max)) { + LOG(INFO) << xy_index; + EXPECT_TRUE((xy_index >= min).all()); + EXPECT_TRUE((xy_index <= max).all()); + ++num_indices; + } + EXPECT_EQ(9, num_indices); +} + +} // namespace +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/CMakeLists.txt b/cartographer/cartographer/mapping_3d/CMakeLists.txt new file mode 100644 index 0000000..394f63a --- /dev/null +++ b/cartographer/cartographer/mapping_3d/CMakeLists.txt @@ -0,0 +1,318 @@ +# 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_3d_acceleration_cost_function + USES_EIGEN + HDRS + acceleration_cost_function.h +) + +google_library(mapping_3d_ceres_pose + USES_CERES + USES_EIGEN + SRCS + ceres_pose.cc + HDRS + ceres_pose.h + DEPENDS + transform_rigid_transform +) + +google_library(mapping_3d_global_trajectory_builder + SRCS + global_trajectory_builder.cc + HDRS + global_trajectory_builder.h + DEPENDS + mapping_3d_local_trajectory_builder + mapping_3d_proto_local_trajectory_builder_options + mapping_3d_sparse_pose_graph + mapping_global_trajectory_builder_interface +) + +google_library(mapping_3d_hybrid_grid + USES_CERES + USES_EIGEN + HDRS + hybrid_grid.h + DEPENDS + common_make_unique + common_math + common_port + mapping_probability_values +) + +google_library(mapping_3d_imu_integration + USES_CERES + USES_EIGEN + SRCS + imu_integration.cc + HDRS + imu_integration.h + DEPENDS + common_time + transform_transform +) + +google_library(mapping_3d_kalman_local_trajectory_builder + USES_CERES + SRCS + kalman_local_trajectory_builder.cc + HDRS + kalman_local_trajectory_builder.h + DEPENDS + common_make_unique + common_time + kalman_filter_pose_tracker + kalman_filter_proto_pose_tracker_options + mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options + mapping_3d_local_trajectory_builder_interface + mapping_3d_motion_filter + mapping_3d_proto_local_trajectory_builder_options + mapping_3d_proto_submaps_options + mapping_3d_scan_matching_ceres_scan_matcher + mapping_3d_scan_matching_proto_ceres_scan_matcher_options + mapping_3d_scan_matching_real_time_correlative_scan_matcher + mapping_3d_submaps + mapping_global_trajectory_builder_interface + sensor_laser + sensor_voxel_filter +) + +google_library(mapping_3d_kalman_local_trajectory_builder_options + SRCS + kalman_local_trajectory_builder_options.cc + HDRS + kalman_local_trajectory_builder_options.h + DEPENDS + common_lua_parameter_dictionary + kalman_filter_pose_tracker + mapping_2d_scan_matching_real_time_correlative_scan_matcher + mapping_3d_proto_kalman_local_trajectory_builder_options +) + +google_library(mapping_3d_laser_fan_inserter + USES_CERES + USES_EIGEN + SRCS + laser_fan_inserter.cc + HDRS + laser_fan_inserter.h + DEPENDS + mapping_3d_hybrid_grid + mapping_3d_proto_laser_fan_inserter_options + mapping_probability_values + sensor_laser + sensor_point_cloud +) + +google_library(mapping_3d_local_trajectory_builder + SRCS + local_trajectory_builder.cc + HDRS + local_trajectory_builder.h + DEPENDS + common_make_unique + mapping_3d_kalman_local_trajectory_builder + mapping_3d_local_trajectory_builder_interface + mapping_3d_optimizing_local_trajectory_builder + mapping_3d_proto_local_trajectory_builder_options +) + +google_library(mapping_3d_local_trajectory_builder_interface + HDRS + local_trajectory_builder_interface.h + DEPENDS + common_time + mapping_3d_submaps + mapping_global_trajectory_builder_interface + sensor_laser + transform_rigid_transform +) + +google_library(mapping_3d_local_trajectory_builder_options + USES_CERES + SRCS + local_trajectory_builder_options.cc + HDRS + local_trajectory_builder_options.h + DEPENDS + common_lua_parameter_dictionary + mapping_3d_kalman_local_trajectory_builder_options + mapping_3d_motion_filter + mapping_3d_optimizing_local_trajectory_builder_options + mapping_3d_proto_local_trajectory_builder_options + mapping_3d_scan_matching_ceres_scan_matcher + mapping_3d_submaps + sensor_voxel_filter +) + +google_library(mapping_3d_motion_filter + USES_CERES + SRCS + motion_filter.cc + HDRS + motion_filter.h + DEPENDS + common_lua_parameter_dictionary + common_time + mapping_3d_proto_motion_filter_options + transform_rigid_transform + transform_transform +) + +google_library(mapping_3d_optimizing_local_trajectory_builder + USES_CERES + SRCS + optimizing_local_trajectory_builder.cc + HDRS + optimizing_local_trajectory_builder.h + DEPENDS + common_ceres_solver_options + common_make_unique + common_time + kalman_filter_pose_tracker + mapping_3d_imu_integration + mapping_3d_local_trajectory_builder_interface + mapping_3d_motion_filter + mapping_3d_proto_local_trajectory_builder_options + mapping_3d_proto_optimizing_local_trajectory_builder_options + mapping_3d_rotation_cost_function + mapping_3d_scan_matching_occupied_space_cost_functor + mapping_3d_scan_matching_proto_ceres_scan_matcher_options + mapping_3d_scan_matching_translation_delta_cost_functor + mapping_3d_submaps + mapping_3d_translation_cost_function + mapping_global_trajectory_builder_interface + sensor_laser + sensor_voxel_filter + transform_rigid_transform + transform_transform + transform_transform_interpolation_buffer +) + +google_library(mapping_3d_optimizing_local_trajectory_builder_options + SRCS + optimizing_local_trajectory_builder_options.cc + HDRS + optimizing_local_trajectory_builder_options.h + DEPENDS + common_lua_parameter_dictionary + mapping_3d_proto_optimizing_local_trajectory_builder_options +) + +google_library(mapping_3d_rotation_cost_function + USES_EIGEN + HDRS + rotation_cost_function.h +) + +google_library(mapping_3d_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_mutex + common_thread_pool + common_time + kalman_filter_pose_tracker + mapping_3d_sparse_pose_graph_constraint_builder + mapping_3d_sparse_pose_graph_optimization_problem + mapping_3d_submaps + mapping_proto_scan_matching_progress + mapping_sparse_pose_graph + mapping_sparse_pose_graph_proto_constraint_builder_options + mapping_trajectory_connectivity + sensor_point_cloud + sensor_voxel_filter + transform_rigid_transform + transform_transform +) + +google_library(mapping_3d_submaps + USES_CERES + USES_EIGEN + SRCS + submaps.cc + HDRS + submaps.h + DEPENDS + common_math + common_port + mapping_2d_laser_fan_inserter + mapping_2d_probability_grid + mapping_3d_hybrid_grid + mapping_3d_laser_fan_inserter + mapping_3d_proto_submaps_options + mapping_sparse_pose_graph + mapping_submaps + sensor_laser + transform_transform +) + +google_library(mapping_3d_translation_cost_function + USES_EIGEN + HDRS + translation_cost_function.h +) + +google_test(mapping_3d_hybrid_grid_test + SRCS + hybrid_grid_test.cc + DEPENDS + mapping_3d_hybrid_grid +) + +google_test(mapping_3d_kalman_local_trajectory_builder_test + USES_CERES + USES_EIGEN + SRCS + kalman_local_trajectory_builder_test.cc + DEPENDS + common_lua_parameter_dictionary_test_helpers + common_time + mapping_3d_hybrid_grid + mapping_3d_kalman_local_trajectory_builder + mapping_3d_local_trajectory_builder_options + sensor_laser + transform_rigid_transform + transform_rigid_transform_test_helpers + transform_transform +) + +google_test(mapping_3d_laser_fan_inserter_test + SRCS + laser_fan_inserter_test.cc + DEPENDS + common_lua_parameter_dictionary_test_helpers + mapping_3d_laser_fan_inserter +) + +google_test(mapping_3d_motion_filter_test + SRCS + motion_filter_test.cc + DEPENDS + common_lua_parameter_dictionary_test_helpers + mapping_3d_motion_filter +) diff --git a/cartographer/cartographer/mapping_3d/acceleration_cost_function.h b/cartographer/cartographer/mapping_3d/acceleration_cost_function.h new file mode 100644 index 0000000..33ddf84 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/acceleration_cost_function.h @@ -0,0 +1,82 @@ +/* + * 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_3D_ACCELERATION_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" + +namespace cartographer { +namespace mapping_3d { + +// Penalizes differences between IMU data and optimized accelerations. +class AccelerationCostFunction { + public: + AccelerationCostFunction(const double scaling_factor, + const Eigen::Vector3d& delta_velocity_imu_frame, + const double first_delta_time_seconds, + const double second_delta_time_seconds) + : scaling_factor_(scaling_factor), + delta_velocity_imu_frame_(delta_velocity_imu_frame), + first_delta_time_seconds_(first_delta_time_seconds), + second_delta_time_seconds_(second_delta_time_seconds) {} + + AccelerationCostFunction(const AccelerationCostFunction&) = delete; + AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete; + + template + bool operator()(const T* const middle_rotation, const T* const start_position, + const T* const middle_position, const T* const end_position, + const T* const gravity_constant, T* residual) const { + const Eigen::Matrix imu_delta_velocity = + ToEigen(middle_rotation) * delta_velocity_imu_frame_.cast() - + *gravity_constant * + (0.5 * (first_delta_time_seconds_ + second_delta_time_seconds_) * + Eigen::Vector3d::UnitZ()) + .cast(); + const Eigen::Matrix start_velocity = + (Eigen::Map>(middle_position) - + Eigen::Map>(start_position)) / + T(first_delta_time_seconds_); + const Eigen::Matrix end_velocity = + (Eigen::Map>(end_position) - + Eigen::Map>(middle_position)) / + T(second_delta_time_seconds_); + const Eigen::Matrix delta_velocity = end_velocity - start_velocity; + + (Eigen::Map>(residual) = + T(scaling_factor_) * (imu_delta_velocity - delta_velocity)); + return true; + } + + private: + template + static Eigen::Quaternion ToEigen(const T* const quaternion) { + return Eigen::Quaternion(quaternion[0], quaternion[1], quaternion[2], + quaternion[3]); + } + + const double scaling_factor_; + const Eigen::Vector3d delta_velocity_imu_frame_; + const double first_delta_time_seconds_; + const double second_delta_time_seconds_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ diff --git a/cartographer/cartographer/mapping_3d/ceres_pose.cc b/cartographer/cartographer/mapping_3d/ceres_pose.cc new file mode 100644 index 0000000..2ca7f83 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/ceres_pose.cc @@ -0,0 +1,43 @@ +/* + * 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_3d/ceres_pose.h" + +namespace cartographer { +namespace mapping_3d { + +CeresPose::CeresPose( + const transform::Rigid3d& rigid, + std::unique_ptr rotation_parametrization, + ceres::Problem* problem) + : translation_({{rigid.translation().x(), rigid.translation().y(), + rigid.translation().z()}}), + rotation_({{rigid.rotation().w(), rigid.rotation().x(), + rigid.rotation().y(), rigid.rotation().z()}}) { + problem->AddParameterBlock(translation_.data(), 3); + problem->AddParameterBlock(rotation_.data(), 4, + rotation_parametrization.release()); +} + +const transform::Rigid3d CeresPose::ToRigid() const { + return transform::Rigid3d( + Eigen::Map(translation_.data()), + Eigen::Quaterniond(rotation_[0], rotation_[1], rotation_[2], + rotation_[3])); +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/ceres_pose.h b/cartographer/cartographer/mapping_3d/ceres_pose.h new file mode 100644 index 0000000..2976128 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/ceres_pose.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ +#define CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping_3d { + +class CeresPose { + public: + CeresPose( + const transform::Rigid3d& rigid, + std::unique_ptr rotation_parametrization, + ceres::Problem* problem); + + CeresPose(const CeresPose&) = delete; + CeresPose& operator=(const CeresPose&) = delete; + + const transform::Rigid3d ToRigid() const; + + double* translation() { return translation_.data(); } + double* rotation() { return rotation_.data(); } + + private: + std::array translation_; + // Rotation quaternion as (w, x, y, z). + std::array rotation_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ diff --git a/cartographer/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/cartographer/mapping_3d/global_trajectory_builder.cc new file mode 100644 index 0000000..77f058f --- /dev/null +++ b/cartographer/cartographer/mapping_3d/global_trajectory_builder.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/mapping_3d/global_trajectory_builder.h" + +#include "cartographer/mapping_3d/local_trajectory_builder.h" + +namespace cartographer { +namespace mapping_3d { + +GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& options, + SparsePoseGraph* sparse_pose_graph) + : sparse_pose_graph_(sparse_pose_graph), + local_trajectory_builder_(CreateLocalTrajectoryBuilder(options)) {} + +GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} + +const mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() const { + return local_trajectory_builder_->submaps(); +} + +mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() { + return local_trajectory_builder_->submaps(); +} + +kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const { + return local_trajectory_builder_->pose_tracker(); +} + +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); + sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity); +} + +void GlobalTrajectoryBuilder::AddLaserFan3D( + const common::Time time, const sensor::LaserFan3D& laser_fan) { + auto insertion_result = + local_trajectory_builder_->AddLaserFan3D(time, laser_fan); + + if (insertion_result == nullptr) { + return; + } + + const int trajectory_node_index = sparse_pose_graph_->AddScan( + insertion_result->time, insertion_result->laser_fan_in_tracking, + insertion_result->pose_observation, insertion_result->covariance_estimate, + insertion_result->submaps, insertion_result->matching_submap, + insertion_result->insertion_submaps); + local_trajectory_builder_->AddTrajectoryNodeIndex(trajectory_node_index); +} + +void GlobalTrajectoryBuilder::AddOdometerPose( + const common::Time time, const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) { + local_trajectory_builder_->AddOdometerPose(time, pose, covariance); +} + +const GlobalTrajectoryBuilder::PoseEstimate& +GlobalTrajectoryBuilder::pose_estimate() const { + return local_trajectory_builder_->pose_estimate(); +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/cartographer/mapping_3d/global_trajectory_builder.h new file mode 100644 index 0000000..79537c5 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/global_trajectory_builder.h @@ -0,0 +1,64 @@ +/* + * 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_3D_GLOBAL_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ + +#include + +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/local_trajectory_builder.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/sparse_pose_graph.h" + +namespace cartographer { +namespace mapping_3d { + +class GlobalTrajectoryBuilder + : public mapping::GlobalTrajectoryBuilderInterface { + public: + GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, + mapping_3d::SparsePoseGraph* sparse_pose_graph); + ~GlobalTrajectoryBuilder() override; + + GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; + GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; + + const mapping_3d::Submaps* submaps() const override; + mapping_3d::Submaps* submaps() override; + kalman_filter::PoseTracker* pose_tracker() const override; + void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) override; + void AddLaserFan3D(common::Time time, + const sensor::LaserFan3D& laser_fan) override; + void AddOdometerPose( + common::Time time, const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) override; + const PoseEstimate& pose_estimate() const override; + + void AddHorizontalLaserFan(common::Time, const sensor::LaserFan3D&) override { + LOG(FATAL) << "Not implemented."; + } + + private: + mapping_3d::SparsePoseGraph* const sparse_pose_graph_; + std::unique_ptr local_trajectory_builder_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/cartographer/mapping_3d/hybrid_grid.h b/cartographer/cartographer/mapping_3d/hybrid_grid.h new file mode 100644 index 0000000..91e9ada --- /dev/null +++ b/cartographer/cartographer/mapping_3d/hybrid_grid.h @@ -0,0 +1,521 @@ +/* + * 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_3D_HYBRID_GRID_H_ +#define CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ + +#include +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping/probability_values.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat +// z-major index. +inline int ToFlatIndex(const Eigen::Array3i& index, const int bits) { + DCHECK((index >= 0).all() && (index < (1 << bits)).all()) << index; + return (((index.z() << bits) + index.y()) << bits) + index.x(); +} + +// Converts a flat z-major 'index' to a 3-dimensional index with each dimension +// from 0 to 2^'bits' - 1. +inline Eigen::Array3i To3DIndex(const int index, const int bits) { + DCHECK_LT(index, 1 << (3 * bits)); + const int mask = (1 << bits) - 1; + return Eigen::Array3i(index & mask, (index >> bits) & mask, + (index >> bits) >> bits); +} + +// A function to compare value to the default value. (Allows specializations). +template +bool IsDefaultValue(const TValueType& v) { + return v == TValueType(); +} + +// Specialization to compare a std::vector to the default value. +template +bool IsDefaultValue(const std::vector& v) { + return v.empty(); +} + +// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of +// type 'ValueType' in contiguous memory. Indices in each dimension are 0-based. +template +class FlatGrid { + public: + using ValueType = TValueType; + + // Creates a new flat grid with all values being default constructed. + FlatGrid() { + for (ValueType& value : cells_) { + value = ValueType(); + } + } + + FlatGrid(const FlatGrid&) = delete; + FlatGrid& operator=(const FlatGrid&) = delete; + + // Returns the number of voxels per dimension. + static int grid_size() { return 1 << kBits; } + + // Returns the value stored at 'index', each dimension of 'index' being + // between 0 and grid_size() - 1. + ValueType value(const Eigen::Array3i& index) const { + return cells_[ToFlatIndex(index, kBits)]; + } + + // Returns a pointer to a value to allow changing it. + ValueType* mutable_value(const Eigen::Array3i& index) { + return &cells_[ToFlatIndex(index, kBits)]; + } + + // An iterator for iterating over all values not comparing equal to the + // default constructed value. + class Iterator { + public: + Iterator() : current_(nullptr), end_(nullptr) {} + + explicit Iterator(const FlatGrid& flat_grid) + : current_(flat_grid.cells_.data()), + end_(flat_grid.cells_.data() + flat_grid.cells_.size()) { + while (!Done() && IsDefaultValue(*current_)) { + ++current_; + } + } + + void Next() { + DCHECK(!Done()); + do { + ++current_; + } while (!Done() && IsDefaultValue(*current_)); + } + + bool Done() const { return current_ == end_; } + + Eigen::Array3i GetCellIndex() const { + DCHECK(!Done()); + const int index = (1 << (3 * kBits)) - (end_ - current_); + return To3DIndex(index, kBits); + } + + const ValueType& GetValue() const { + DCHECK(!Done()); + return *current_; + } + + private: + const ValueType* current_; + const ValueType* end_; + }; + + private: + std::array cells_; +}; + +// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type +// 'WrappedGrid'. Wrapped grids are constructed on first access via +// 'mutable_value()'. +template +class NestedGrid { + public: + using ValueType = typename WrappedGrid::ValueType; + + // Returns the number of voxels per dimension. + static int grid_size() { return WrappedGrid::grid_size() << kBits; } + + // Returns the value stored at 'index', each dimension of 'index' being + // between 0 and grid_size() - 1. + ValueType value(const Eigen::Array3i& index) const { + const Eigen::Array3i meta_index = GetMetaIndex(index); + const WrappedGrid* const meta_cell = + meta_cells_[ToFlatIndex(meta_index, kBits)].get(); + if (meta_cell == nullptr) { + return ValueType(); + } + const Eigen::Array3i inner_index = + index - meta_index * WrappedGrid::grid_size(); + return meta_cell->value(inner_index); + } + + // Returns a pointer to the value at 'index' to allow changing it. If + // necessary a new wrapped grid is constructed to contain that value. + ValueType* mutable_value(const Eigen::Array3i& index) { + const Eigen::Array3i meta_index = GetMetaIndex(index); + std::unique_ptr& meta_cell = + meta_cells_[ToFlatIndex(meta_index, kBits)]; + if (meta_cell == nullptr) { + meta_cell = common::make_unique(); + } + const Eigen::Array3i inner_index = + index - meta_index * WrappedGrid::grid_size(); + return meta_cell->mutable_value(inner_index); + } + + // An iterator for iterating over all values not comparing equal to the + // default constructed value. + class Iterator { + public: + Iterator() : current_(nullptr), end_(nullptr), nested_iterator_() {} + + explicit Iterator(const NestedGrid& nested_grid) + : current_(nested_grid.meta_cells_.data()), + end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()), + nested_iterator_() { + AdvanceToValidNestedIterator(); + } + + void Next() { + DCHECK(!Done()); + nested_iterator_.Next(); + if (!nested_iterator_.Done()) { + return; + } + ++current_; + AdvanceToValidNestedIterator(); + } + + bool Done() const { return current_ == end_; } + + Eigen::Array3i GetCellIndex() const { + DCHECK(!Done()); + const int index = (1 << (3 * kBits)) - (end_ - current_); + return To3DIndex(index, kBits) * WrappedGrid::grid_size() + + nested_iterator_.GetCellIndex(); + } + + const ValueType& GetValue() const { + DCHECK(!Done()); + return nested_iterator_.GetValue(); + } + + private: + void AdvanceToValidNestedIterator() { + for (; !Done(); ++current_) { + if (*current_ != nullptr) { + nested_iterator_ = typename WrappedGrid::Iterator(**current_); + if (!nested_iterator_.Done()) { + break; + } + } + } + } + + const std::unique_ptr* current_; + const std::unique_ptr* end_; + typename WrappedGrid::Iterator nested_iterator_; + }; + + private: + // Returns the Eigen::Array3i (meta) index of the meta cell containing + // 'index'. + Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const { + DCHECK((index >= 0).all()) << index; + const Eigen::Array3i meta_index = index / WrappedGrid::grid_size(); + DCHECK((meta_index < (1 << kBits)).all()) << index; + return meta_index; + } + + std::array, 1 << (3 * kBits)> meta_cells_; +}; + +// A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped +// grids are constructed on first access via 'mutable_value()'. If necessary, +// the grid grows to twice the size in each dimension. The range of indices is +// (almost) symmetric around the origin, i.e. negative indices are allowed. +template +class DynamicGrid { + public: + using ValueType = typename WrappedGrid::ValueType; + + DynamicGrid() : bits_(1), meta_cells_(8) {} + DynamicGrid(DynamicGrid&&) = default; + DynamicGrid& operator=(DynamicGrid&&) = default; + + // Returns the current number of voxels per dimension. + int grid_size() const { return WrappedGrid::grid_size() << bits_; } + + // Returns the value stored at 'index'. + ValueType value(const Eigen::Array3i& index) const { + const Eigen::Array3i shifted_index = index + (grid_size() >> 1); + // The cast to unsigned is for performance to check with 3 comparisons + // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size. + if ((shifted_index.cast() >= grid_size()).any()) { + return ValueType(); + } + const Eigen::Array3i meta_index = GetMetaIndex(shifted_index); + const WrappedGrid* const meta_cell = + meta_cells_[ToFlatIndex(meta_index, bits_)].get(); + if (meta_cell == nullptr) { + return ValueType(); + } + const Eigen::Array3i inner_index = + shifted_index - meta_index * WrappedGrid::grid_size(); + return meta_cell->value(inner_index); + } + + // Returns a pointer to the value at 'index' to allow changing it, dynamically + // growing the DynamicGrid and constructing new WrappedGrids as needed. + ValueType* mutable_value(const Eigen::Array3i& index) { + const Eigen::Array3i shifted_index = index + (grid_size() >> 1); + // The cast to unsigned is for performance to check with 3 comparisons + // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size. + if ((shifted_index.cast() >= grid_size()).any()) { + Grow(); + return mutable_value(index); + } + const Eigen::Array3i meta_index = GetMetaIndex(shifted_index); + std::unique_ptr& meta_cell = + meta_cells_[ToFlatIndex(meta_index, bits_)]; + if (meta_cell == nullptr) { + meta_cell = common::make_unique(); + } + const Eigen::Array3i inner_index = + shifted_index - meta_index * WrappedGrid::grid_size(); + return meta_cell->mutable_value(inner_index); + } + + // An iterator for iterating over all values not comparing equal to the + // default constructed value. + class Iterator { + public: + explicit Iterator(const DynamicGrid& dynamic_grid) + : bits_(dynamic_grid.bits_), + current_(dynamic_grid.meta_cells_.data()), + end_(dynamic_grid.meta_cells_.data() + + dynamic_grid.meta_cells_.size()), + nested_iterator_() { + AdvanceToValidNestedIterator(); + } + + void Next() { + DCHECK(!Done()); + nested_iterator_.Next(); + if (!nested_iterator_.Done()) { + return; + } + ++current_; + AdvanceToValidNestedIterator(); + } + + bool Done() const { return current_ == end_; } + + Eigen::Array3i GetCellIndex() const { + DCHECK(!Done()); + const int outer_index = (1 << (3 * bits_)) - (end_ - current_); + const Eigen::Array3i shifted_index = + To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() + + nested_iterator_.GetCellIndex(); + return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size()); + } + + const ValueType& GetValue() const { + DCHECK(!Done()); + return nested_iterator_.GetValue(); + } + + void AdvanceToEnd() { current_ = end_; } + + const std::pair operator*() const { + return std::pair(GetCellIndex(), GetValue()); + } + + Iterator& operator++() { + Next(); + return *this; + } + + bool operator!=(const Iterator& it) const { + return it.current_ != current_; + } + + private: + void AdvanceToValidNestedIterator() { + for (; !Done(); ++current_) { + if (*current_ != nullptr) { + nested_iterator_ = typename WrappedGrid::Iterator(**current_); + if (!nested_iterator_.Done()) { + break; + } + } + } + } + + int bits_; + const std::unique_ptr* current_; + const std::unique_ptr* const end_; + typename WrappedGrid::Iterator nested_iterator_; + }; + + private: + // Returns the Eigen::Array3i (meta) index of the meta cell containing + // 'index'. + Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const { + DCHECK((index >= 0).all()) << index; + const Eigen::Array3i meta_index = index / WrappedGrid::grid_size(); + DCHECK((meta_index < (1 << bits_)).all()) << index; + return meta_index; + } + + // Grows this grid by a factor of 2 in each of the 3 dimensions. + void Grow() { + const int new_bits = bits_ + 1; + CHECK_LE(new_bits, 8); + std::vector> new_meta_cells_( + 8 * meta_cells_.size()); + for (int z = 0; z != (1 << bits_); ++z) { + for (int y = 0; y != (1 << bits_); ++y) { + for (int x = 0; x != (1 << bits_); ++x) { + const Eigen::Array3i original_meta_index(x, y, z); + const Eigen::Array3i new_meta_index = + original_meta_index + (1 << (bits_ - 1)); + new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] = + std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]); + } + } + } + meta_cells_ = std::move(new_meta_cells_); + bits_ = new_bits; + } + + int bits_; + std::vector> meta_cells_; +}; + +template +using Grid = DynamicGrid, 3>>; + +// Represents a 3D grid as a wide, shallow tree. +template +class HybridGridBase : public Grid { + public: + using Iterator = typename Grid::Iterator; + + // Creates a new tree-based probability grid around 'origin' which becomes the + // center of the cell at index (0, 0, 0). Each voxel has edge length + // 'resolution'. + HybridGridBase(const float resolution, const Eigen::Vector3f& origin) + : resolution_(resolution), origin_(origin) {} + + float resolution() const { return resolution_; } + Eigen::Vector3f origin() const { return origin_; } + + // Returns the index of the cell containing the 'point'. Indices are integer + // vectors identifying cells, for this the relative distance from the origin + // is rounded to the next multiple of the resolution. + Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const { + Eigen::Array3f index = (point - origin_).array() / resolution_; + return Eigen::Array3i(common::RoundToInt(index.x()), + common::RoundToInt(index.y()), + common::RoundToInt(index.z())); + } + + // Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1). + static Eigen::Array3i GetOctant(const int i) { + DCHECK_GE(i, 0); + DCHECK_LT(i, 8); + return Eigen::Array3i(static_cast(i & 1), static_cast(i & 2), + static_cast(i & 4)); + } + + // Returns the center of the cell at 'index'. + Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const { + return index.matrix().cast() * resolution_ + origin_; + } + + // Iterator functions for range-based for loops. + Iterator begin() const { return Iterator(*this); } + + Iterator end() const { + Iterator it(*this); + it.AdvanceToEnd(); + return it; + } + + private: + // Edge length of each voxel. + const float resolution_; + + // Position of the center of the octree. + const Eigen::Vector3f origin_; +}; + +// A grid containing probability values stored using 15 bits, and an update +// marker per voxel. +class HybridGrid : public HybridGridBase { + public: + HybridGrid(const float resolution, const Eigen::Vector3f& origin) + : HybridGridBase(resolution, origin) {} + + // Sets the probability of the cell at 'index' to the given 'probability'. + void SetProbability(const Eigen::Array3i& index, const float probability) { + *mutable_value(index) = mapping::ProbabilityToValue(probability); + } + + // Starts the next update sequence. + void StartUpdate() { + while (!update_indices_.empty()) { + DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker); + *update_indices_.back() -= mapping::kUpdateMarker; + update_indices_.pop_back(); + } + } + + // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() + // to the probability of the cell at '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::Array3i& index, + const std::vector& table) { + DCHECK_EQ(table.size(), mapping::kUpdateMarker); + uint16* const cell = mutable_value(index); + if (*cell >= mapping::kUpdateMarker) { + return false; + } + update_indices_.push_back(cell); + *cell = table[*cell]; + DCHECK_GE(*cell, mapping::kUpdateMarker); + return true; + } + + // Returns the probability of the cell with 'index'. + float GetProbability(const Eigen::Array3i& index) const { + return mapping::ValueToProbability(value(index)); + } + + // Returns true if the probability at the specified 'index' is known. + bool IsKnown(const Eigen::Array3i& index) const { return value(index) != 0; } + + private: + // Markers at changed cells. + std::vector update_indices_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ diff --git a/cartographer/cartographer/mapping_3d/hybrid_grid_test.cc b/cartographer/cartographer/mapping_3d/hybrid_grid_test.cc new file mode 100644 index 0000000..6b29c34 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/hybrid_grid_test.cc @@ -0,0 +1,182 @@ +/* + * 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_3d/hybrid_grid.h" + +#include +#include + +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_3d { +namespace { + +TEST(HybridGridTest, ApplyOdds) { + HybridGrid hybrid_grid(1.f, Eigen::Vector3f(-0.5f, -0.5f, -0.5f)); + + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 0))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 1))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 1))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 1))); + EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 1))); + + hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f); + + hybrid_grid.StartUpdate(); + hybrid_grid.ApplyLookupTable( + Eigen::Array3i(1, 0, 1), + mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); + EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f); + + hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f); + + hybrid_grid.StartUpdate(); + hybrid_grid.ApplyLookupTable( + Eigen::Array3i(0, 1, 0), + mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f))); + EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f); + + // Tests adding odds to an unknown cell. + hybrid_grid.StartUpdate(); + hybrid_grid.ApplyLookupTable( + Eigen::Array3i(1, 1, 1), + mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f))); + EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); + + // Tests that further updates are ignored if StartUpdate() isn't called. + hybrid_grid.ApplyLookupTable( + Eigen::Array3i(1, 1, 1), + mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); + EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); + hybrid_grid.StartUpdate(); + hybrid_grid.ApplyLookupTable( + Eigen::Array3i(1, 1, 1), + mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); + EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f); +} + +TEST(HybridGridTest, GetProbability) { + HybridGrid hybrid_grid(1.f, Eigen::Vector3f(-0.5f, -0.5f, -0.5f)); + + hybrid_grid.SetProbability( + hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 0.5f, 0.5f)), + mapping::kMaxProbability); + EXPECT_NEAR(hybrid_grid.GetProbability( + hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 0.5f, 0.5f))), + mapping::kMaxProbability, 1e-6); + for (const Eigen::Array3i& index : + {hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 1.5, 0.5f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(.5f, 0.5, 0.5f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(0.5f, 1.5, 0.5f))}) { + EXPECT_FALSE(hybrid_grid.IsKnown(index)); + } +} + +MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); } + +TEST(HybridGridTest, GetCellIndex) { + HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -13.f, -2.f)); + + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-7.f, -13.f, -2.f)), + AllCwiseEqual(Eigen::Array3i(0, 0, 0))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-7.f, 13.f, 8.f)), + AllCwiseEqual(Eigen::Array3i(0, 13, 5))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.f, -13.f, 8.f)), + AllCwiseEqual(Eigen::Array3i(7, 0, 5))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.f, 13.f, -2.f)), + AllCwiseEqual(Eigen::Array3i(7, 13, 0))); + + // Check around the origin. + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(1.5f, -1.5f, -1.5f)), + AllCwiseEqual(Eigen::Array3i(4, 6, 0))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.5f, -0.5f, -0.5f)), + AllCwiseEqual(Eigen::Array3i(4, 6, 1))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 1.5f, 0.5f)), + AllCwiseEqual(Eigen::Array3i(3, 7, 1))); + EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-1.5f, 0.5f, 1.5f)), + AllCwiseEqual(Eigen::Array3i(3, 7, 2))); +} + +TEST(HybridGridTest, GetCenterOfCell) { + HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -13.f, -2.f)); + + const Eigen::Array3i index(3, 2, 1); + const Eigen::Vector3f center = hybrid_grid.GetCenterOfCell(index); + EXPECT_NEAR(-1.f, center.x(), 1e-6); + EXPECT_NEAR(-9.f, center.y(), 1e-6); + EXPECT_NEAR(0.f, center.z(), 1e-6); + EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index)); +} + +TEST(HybridGridTest, TestIteration) { + HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f)); + + std::map, float> values; + std::mt19937 rng(1285120005); + std::uniform_real_distribution value_distribution( + mapping::kMinProbability, mapping::kMaxProbability); + std::uniform_int_distribution xyz_distribution(-3000, 2999); + for (int i = 0; i < 10000; ++i) { + const auto x = xyz_distribution(rng); + const auto y = xyz_distribution(rng); + const auto z = xyz_distribution(rng); + values.emplace(std::make_tuple(x, y, z), value_distribution(rng)); + } + + for (const auto& pair : values) { + const Eigen::Array3i cell_index(std::get<0>(pair.first), + std::get<1>(pair.first), + std::get<2>(pair.first)); + hybrid_grid.SetProbability(cell_index, pair.second); + } + + for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { + const Eigen::Array3i cell_index = it.GetCellIndex(); + const float iterator_probability = + mapping::ValueToProbability(it.GetValue()); + EXPECT_EQ(iterator_probability, hybrid_grid.GetProbability(cell_index)); + const std::tuple key = + std::make_tuple(cell_index[0], cell_index[1], cell_index[2]); + EXPECT_TRUE(values.count(key)); + EXPECT_NEAR(values[key], iterator_probability, 1e-4); + values.erase(key); + } + + // Test that range based loop is equivalent to using the iterator. + auto it = HybridGrid::Iterator(hybrid_grid); + for (const auto& cell : hybrid_grid) { + ASSERT_FALSE(it.Done()); + EXPECT_THAT(cell.first, AllCwiseEqual(it.GetCellIndex())); + EXPECT_EQ(cell.second, it.GetValue()); + it.Next(); + } + + // Now 'values' must not contain values. + for (const auto& pair : values) { + const Eigen::Array3i cell_index(std::get<0>(pair.first), + std::get<1>(pair.first), + std::get<2>(pair.first)); + ADD_FAILURE() << cell_index << " Probability: " << pair.second; + } +} + +} // namespace +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/imu_integration.cc b/cartographer/cartographer/mapping_3d/imu_integration.cc new file mode 100644 index 0000000..542e7da --- /dev/null +++ b/cartographer/cartographer/mapping_3d/imu_integration.cc @@ -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. + */ + +#include "cartographer/mapping_3d/imu_integration.h" + +namespace cartographer { +namespace mapping_3d { + +IntegrateImuResult IntegrateImu( + const std::deque& imu_data, const common::Time start_time, + const common::Time end_time, std::deque::const_iterator* it) { + return IntegrateImu(imu_data, Eigen::Affine3d::Identity(), + Eigen::Affine3d::Identity(), start_time, end_time, + it); +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/imu_integration.h b/cartographer/cartographer/mapping_3d/imu_integration.h new file mode 100644 index 0000000..1c63767 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/imu_integration.h @@ -0,0 +1,96 @@ +/* + * 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_3D_IMU_INTEGRATION_H_ +#define CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_ + +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/time.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +struct ImuData { + common::Time time; + Eigen::Vector3d linear_acceleration; + Eigen::Vector3d angular_velocity; +}; + +template +struct IntegrateImuResult { + Eigen::Matrix delta_velocity; + Eigen::Quaternion delta_rotation; +}; + +// Returns velocity delta in map frame. +IntegrateImuResult IntegrateImu( + const std::deque& imu_data, const common::Time start_time, + const common::Time end_time, std::deque::const_iterator* it); + +template +IntegrateImuResult IntegrateImu( + const std::deque& imu_data, + const Eigen::Transform& + linear_acceleration_calibration, + const Eigen::Transform& angular_velocity_calibration, + const common::Time start_time, const common::Time end_time, + std::deque::const_iterator* it) { + CHECK_LE(start_time, end_time); + CHECK(*it != imu_data.cend()); + CHECK_LE((*it)->time, start_time); + if ((*it) + 1 != imu_data.cend()) { + CHECK_GT(((*it) + 1)->time, start_time); + } + + common::Time current_time = start_time; + + IntegrateImuResult result = {Eigen::Matrix::Zero(), + Eigen::Quaterniond::Identity().cast()}; + while (current_time < end_time) { + common::Time next_imu_data = common::Time::max(); + if ((*it) + 1 != imu_data.cend()) { + next_imu_data = ((*it) + 1)->time; + } + common::Time next_time = std::min(next_imu_data, end_time); + const T delta_t(common::ToSeconds(next_time - current_time)); + + const Eigen::Matrix delta_angle = + (angular_velocity_calibration * (*it)->angular_velocity.cast()) * + delta_t; + result.delta_rotation *= + transform::AngleAxisVectorToRotationQuaternion(delta_angle); + result.delta_velocity += + result.delta_rotation * ((linear_acceleration_calibration * + (*it)->linear_acceleration.cast()) * + delta_t); + current_time = next_time; + if (current_time == next_imu_data) { + ++(*it); + } + } + return result; +} + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_ diff --git a/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder.cc new file mode 100644 index 0000000..7f3d829 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -0,0 +1,238 @@ +/* + * 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_3d/kalman_local_trajectory_builder.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h" +#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/proto/submaps_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& options) + : options_(options), + submaps_(common::make_unique(options.submaps_options())), + scan_matcher_pose_estimate_(transform::Rigid3d::Identity()), + motion_filter_(options.motion_filter_options()), + real_time_correlative_scan_matcher_( + common::make_unique( + options_.kalman_local_trajectory_builder_options() + .real_time_correlative_scan_matcher_options())), + ceres_scan_matcher_(common::make_unique( + options_.ceres_scan_matcher_options())), + num_accumulated_(0), + first_pose_prediction_(transform::Rigid3f::Identity()), + accumulated_laser_fan_{Eigen::Vector3f::Zero(), {}, {}} {} + +KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {} + +mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() { + return submaps_.get(); +} + +kalman_filter::PoseTracker* KalmanLocalTrajectoryBuilder::pose_tracker() const { + return pose_tracker_.get(); +} + +void KalmanLocalTrajectoryBuilder::AddImuData( + const common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) { + if (!pose_tracker_) { + pose_tracker_ = common::make_unique( + options_.kalman_local_trajectory_builder_options() + .pose_tracker_options(), + kalman_filter::PoseTracker::ModelFunction::k3D, 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); +} + +std::unique_ptr +KalmanLocalTrajectoryBuilder::AddLaserFan3D( + const common::Time time, const sensor::LaserFan3D& laser_fan) { + if (!pose_tracker_) { + LOG(INFO) << "PoseTracker not yet initialized."; + return nullptr; + } + + transform::Rigid3d pose_prediction; + kalman_filter::PoseCovariance unused_covariance_prediction; + pose_tracker_->GetPoseEstimateMeanAndCovariance( + time, &pose_prediction, &unused_covariance_prediction); + if (num_accumulated_ == 0) { + first_pose_prediction_ = pose_prediction.cast(); + accumulated_laser_fan_ = + sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}}; + } + + const transform::Rigid3f tracking_delta = + first_pose_prediction_.inverse() * pose_prediction.cast(); + const sensor::LaserFan3D laser_fan_in_first_tracking = + sensor::TransformLaserFan3D(laser_fan, tracking_delta); + for (const Eigen::Vector3f& laser_return : + laser_fan_in_first_tracking.returns) { + const Eigen::Vector3f delta = + laser_return - laser_fan_in_first_tracking.origin; + const float range = delta.norm(); + if (range >= options_.laser_min_range()) { + if (range <= options_.laser_max_range()) { + accumulated_laser_fan_.returns.push_back(laser_return); + } else { + // We insert a ray cropped to 'laser_max_range' as a miss for hits + // beyond the maximum range. This way the free space up to the maximum + // range will be updated. + accumulated_laser_fan_.misses.push_back( + laser_fan_in_first_tracking.origin + + options_.laser_max_range() / range * delta); + } + } + } + ++num_accumulated_; + + if (num_accumulated_ >= options_.scans_per_accumulation()) { + num_accumulated_ = 0; + return AddAccumulatedLaserFan3D( + time, sensor::TransformLaserFan3D(accumulated_laser_fan_, + tracking_delta.inverse())); + } + return nullptr; +} + +std::unique_ptr +KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan3D( + const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking) { + const sensor::LaserFan3D filtered_laser_fan = { + laser_fan_in_tracking.origin, + sensor::VoxelFiltered(laser_fan_in_tracking.returns, + options_.laser_voxel_filter_size()), + sensor::VoxelFiltered(laser_fan_in_tracking.misses, + options_.laser_voxel_filter_size())}; + + if (filtered_laser_fan.returns.empty()) { + LOG(WARNING) << "Dropped empty laser scanner point cloud."; + return nullptr; + } + + transform::Rigid3d pose_prediction; + kalman_filter::PoseCovariance covariance_prediction; + pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_prediction, + &covariance_prediction); + + transform::Rigid3d initial_ceres_pose = pose_prediction; + sensor::AdaptiveVoxelFilter adaptive_voxel_filter( + options_.high_resolution_adaptive_voxel_filter_options()); + const sensor::PointCloud filtered_point_cloud_in_tracking = + adaptive_voxel_filter.Filter(filtered_laser_fan.returns); + if (options_.kalman_local_trajectory_builder_options() + .use_online_correlative_scan_matching()) { + real_time_correlative_scan_matcher_->Match( + pose_prediction, filtered_point_cloud_in_tracking, + submaps_->high_resolution_matching_grid(), &initial_ceres_pose); + } + + transform::Rigid3d pose_observation; + kalman_filter::PoseCovariance covariance_observation; + ceres::Solver::Summary summary; + + sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( + options_.low_resolution_adaptive_voxel_filter_options()); + const sensor::PointCloud low_resolution_point_cloud_in_tracking = + low_resolution_adaptive_voxel_filter.Filter(filtered_laser_fan.returns); + ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose, + {{&filtered_point_cloud_in_tracking, + &submaps_->high_resolution_matching_grid()}, + {&low_resolution_point_cloud_in_tracking, + &submaps_->low_resolution_matching_grid()}}, + &pose_observation, &covariance_observation, + &summary); + pose_tracker_->AddPoseObservation(time, pose_observation, + covariance_observation); + + kalman_filter::PoseCovariance covariance_estimate; + pose_tracker_->GetPoseEstimateMeanAndCovariance( + time, &scan_matcher_pose_estimate_, &covariance_estimate); + + last_pose_estimate_ = { + time, + {pose_prediction, covariance_prediction}, + {pose_observation, covariance_observation}, + {scan_matcher_pose_estimate_, covariance_estimate}, + scan_matcher_pose_estimate_, + sensor::TransformPointCloud(filtered_laser_fan.returns, + pose_observation.cast())}; + + return InsertIntoSubmap(time, filtered_laser_fan, pose_observation, + covariance_estimate); +} + +void KalmanLocalTrajectoryBuilder::AddOdometerPose( + const common::Time time, const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) { + if (!pose_tracker_) { + pose_tracker_.reset(new kalman_filter::PoseTracker( + options_.kalman_local_trajectory_builder_options() + .pose_tracker_options(), + kalman_filter::PoseTracker::ModelFunction::k3D, time)); + } + pose_tracker_->AddOdometerPoseObservation(time, pose, covariance); +} + +const KalmanLocalTrajectoryBuilder::PoseEstimate& +KalmanLocalTrajectoryBuilder::pose_estimate() const { + return last_pose_estimate_; +} + +void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex( + int trajectory_node_index) { + submaps_->AddTrajectoryNodeIndex(trajectory_node_index); +} + +std::unique_ptr +KalmanLocalTrajectoryBuilder::InsertIntoSubmap( + const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const transform::Rigid3d& pose_observation, + const kalman_filter::PoseCovariance& covariance_estimate) { + if (motion_filter_.IsSimilar(time, pose_observation)) { + return nullptr; + } + const Submap* const matching_submap = + submaps_->Get(submaps_->matching_index()); + std::vector insertion_submaps; + for (int insertion_index : submaps_->insertion_indices()) { + insertion_submaps.push_back(submaps_->Get(insertion_index)); + } + submaps_->InsertLaserFan(sensor::TransformLaserFan3D( + laser_fan_in_tracking, pose_observation.cast())); + return std::unique_ptr(new InsertionResult{ + time, laser_fan_in_tracking, pose_observation, covariance_estimate, + submaps_.get(), matching_submap, insertion_submaps}); +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder.h new file mode 100644 index 0000000..cbc6c76 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -0,0 +1,94 @@ +/* + * 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_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ + +#include + +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/local_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/motion_filter.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/voxel_filter.h" + +namespace cartographer { +namespace mapping_3d { + +// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop +// closure. +class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { + public: + explicit KalmanLocalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& options); + ~KalmanLocalTrajectoryBuilder() override; + + KalmanLocalTrajectoryBuilder(const KalmanLocalTrajectoryBuilder&) = delete; + KalmanLocalTrajectoryBuilder& operator=(const KalmanLocalTrajectoryBuilder&) = + delete; + + void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) override; + std::unique_ptr AddLaserFan3D( + common::Time time, const sensor::LaserFan3D& laser_fan) override; + void AddOdometerPose( + common::Time time, const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) override; + + void AddTrajectoryNodeIndex(int trajectory_node_index) override; + mapping_3d::Submaps* submaps() override; + const PoseEstimate& pose_estimate() const override; + kalman_filter::PoseTracker* pose_tracker() const override; + + private: + std::unique_ptr AddAccumulatedLaserFan3D( + common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking); + + std::unique_ptr InsertIntoSubmap( + const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const transform::Rigid3d& pose_observation, + const kalman_filter::PoseCovariance& covariance_estimate); + + const proto::LocalTrajectoryBuilderOptions options_; + std::unique_ptr submaps_; + + PoseEstimate last_pose_estimate_; + + // Pose of the last computed scan match. + transform::Rigid3d scan_matcher_pose_estimate_; + + MotionFilter motion_filter_; + std::unique_ptr + real_time_correlative_scan_matcher_; + std::unique_ptr ceres_scan_matcher_; + + std::unique_ptr pose_tracker_; + + int num_accumulated_; + transform::Rigid3f first_pose_prediction_; + sensor::LaserFan3D accumulated_laser_fan_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc new file mode 100644 index 0000000..72ad5f0 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc @@ -0,0 +1,43 @@ +/* + * 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_3d/kalman_local_trajectory_builder_options.h" + +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" + +namespace cartographer { +namespace mapping_3d { + +proto::KalmanLocalTrajectoryBuilderOptions +CreateKalmanLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::KalmanLocalTrajectoryBuilderOptions options; + options.set_use_online_correlative_scan_matching( + parameter_dictionary->GetBool("use_online_correlative_scan_matching")); + *options.mutable_real_time_correlative_scan_matcher_options() = + mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary + ->GetDictionary("real_time_correlative_scan_matcher") + .get()); + *options.mutable_pose_tracker_options() = + kalman_filter::CreatePoseTrackerOptions( + parameter_dictionary->GetDictionary("pose_tracker").get()); + return options; +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_options.h b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_options.h new file mode 100644 index 0000000..4ec7a6e --- /dev/null +++ b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_options.h @@ -0,0 +1,33 @@ +/* + * 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_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.pb.h" + +namespace cartographer { +namespace mapping_3d { + +proto::KalmanLocalTrajectoryBuilderOptions +CreateKalmanLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ diff --git a/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc new file mode 100644 index 0000000..5f8641e --- /dev/null +++ b/cartographer/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -0,0 +1,283 @@ +/* + * 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_3d/kalman_local_trajectory_builder.h" + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/local_trajectory_builder_options.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_3d { +namespace { + +class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { + protected: + struct TrajectoryNode { + common::Time time; + transform::Rigid3d pose; + }; + + void SetUp() override { GenerateBubbles(); } + + proto::LocalTrajectoryBuilderOptions CreateTrajectoryBuilderOptions() { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + laser_min_range = 0.5, + laser_max_range = 50., + scans_per_accumulation = 1, + laser_voxel_filter_size = 0.05, + + high_resolution_adaptive_voxel_filter = { + max_length = 0.7, + min_num_points = 200, + max_range = 50., + }, + + low_resolution_adaptive_voxel_filter = { + max_length = 0.7, + min_num_points = 200, + max_range = 50., + }, + + ceres_scan_matcher = { + occupied_space_cost_functor_weight_0 = 5., + occupied_space_cost_functor_weight_1 = 20., + previous_pose_translation_delta_cost_functor_weight = 0.1, + initial_pose_estimate_rotation_delta_cost_functor_weight = 0.3, + covariance_scale = 1e-1, + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 20, + num_threads = 1, + }, + }, + motion_filter = { + max_time_seconds = 0.2, + max_distance_meters = 0.02, + max_angle_radians = 0.001, + }, + submaps = { + high_resolution = 0.2, + high_resolution_max_range = 50., + low_resolution = 0.5, + num_laser_fans = 45000, + laser_fan_inserter = { + hit_probability = 0.7, + miss_probability = 0.4, + num_free_space_voxels = 0, + }, + }, + + use = "KALMAN", + kalman_local_trajectory_builder = { + use_online_correlative_scan_matching = false, + real_time_correlative_scan_matcher = { + linear_search_window = 0.2, + angular_search_window = math.rad(1.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1., + covariance_scale = 1., + }, + pose_tracker = { + orientation_model_variance = 5e-4, + position_model_variance = 0.000654766, + velocity_model_variance = 0.053926, + imu_gravity_time_constant = 1., + imu_gravity_variance = 1e-4, + num_odometry_states = 1, + }, + }, + optimizing_local_trajectory_builder = { + high_resolution_grid_scale = 5., + low_resolution_grid_scale = 15., + velocity_scale = 4e1, + translation_scale = 1e2, + rotation_scale = 1e3, + odometry_translation_scale = 1e4, + odometry_rotation_scale = 1e4, + }, + } + )text"); + return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get()); + } + + void GenerateBubbles() { + std::mt19937 prng(42); + std::uniform_real_distribution distribution(-1.f, 1.f); + + for (int i = 0; i != 100; ++i) { + const float x = distribution(prng); + const float y = distribution(prng); + const float z = distribution(prng); + bubbles_.push_back(10. * Eigen::Vector3f(x, y, z).normalized()); + } + } + + // Computes the earliest intersection of the ray 'from' to 'to' with the + // axis-aligned cube with edge length 30 and center at the origin. Assumes + // that 'from' is inside the cube. + Eigen::Vector3f CollideWithBox(const Eigen::Vector3f& from, + const Eigen::Vector3f& to) { + float first = 100.f; + if (to.x() > from.x()) { + first = std::min(first, (15.f - from.x()) / (to.x() - from.x())); + } else if (to.x() < from.x()) { + first = std::min(first, (-15.f - from.x()) / (to.x() - from.x())); + } + if (to.y() > from.y()) { + first = std::min(first, (15.f - from.y()) / (to.y() - from.y())); + } else if (to.y() < from.y()) { + first = std::min(first, (-15.f - from.y()) / (to.y() - from.y())); + } + if (to.z() > from.z()) { + first = std::min(first, (15.f - from.z()) / (to.z() - from.z())); + } else if (to.z() < from.z()) { + first = std::min(first, (-15.f - from.z()) / (to.z() - from.z())); + } + return first * (to - from) + from; + } + + // Computes the earliest intersection of the ray 'from' to 'to' with all + // bubbles. Returns 'to' if no intersection exists. + Eigen::Vector3f CollideWithBubbles(const Eigen::Vector3f& from, + const Eigen::Vector3f& to) { + float first = 1.f; + constexpr float kBubbleRadius = 0.5f; + for (const Eigen::Vector3f& center : bubbles_) { + const float a = (to - from).squaredNorm(); + const float beta = (to - from).dot(from - center); + const float c = + (from - center).squaredNorm() - kBubbleRadius * kBubbleRadius; + const float discriminant = beta * beta - a * c; + if (discriminant < 0.f) { + continue; + } + const float solution = (-beta - std::sqrt(discriminant)) / a; + if (solution < 0.f) { + continue; + } + first = std::min(first, solution); + } + return first * (to - from) + from; + } + + sensor::LaserFan3D GenerateLaserFan(const transform::Rigid3d& pose) { + // 360 degree rays at 16 angles. + sensor::PointCloud directions_in_laser_frame; + for (int r = -8; r != 8; ++r) { + for (int s = -250; s != 250; ++s) { + directions_in_laser_frame.push_back( + Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX()); + // Second orthogonal laser. + directions_in_laser_frame.push_back( + Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) * + Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX()); + } + } + // We simulate a 30 m edge length box around the origin, also containing + // 50 cm radius spheres. + sensor::PointCloud returns_in_world_frame; + for (const Eigen::Vector3f& direction_in_world_frame : + sensor::TransformPointCloud(directions_in_laser_frame, + pose.cast())) { + const Eigen::Vector3f laser_origin = + pose.cast() * Eigen::Vector3f::Zero(); + returns_in_world_frame.push_back(CollideWithBubbles( + laser_origin, + CollideWithBox(laser_origin, direction_in_world_frame))); + } + return {Eigen::Vector3f::Zero(), + sensor::TransformPointCloud(returns_in_world_frame, + pose.inverse().cast()), + {}}; + } + + void AddLinearOnlyImuObservation(const common::Time time, + const transform::Rigid3d& expected_pose) { + const Eigen::Vector3d gravity = + expected_pose.rotation().inverse() * Eigen::Vector3d(0., 0., 9.81); + local_trajectory_builder_->AddImuData(time, gravity, + Eigen::Vector3d::Zero()); + } + + std::vector GenerateCorkscrewTrajectory() { + std::vector trajectory; + common::Time current_time = common::FromUniversal(12345678); + // One second at zero velocity. + for (int i = 0; i != 5; ++i) { + current_time += common::FromSeconds(0.3); + trajectory.push_back( + TrajectoryNode{current_time, transform::Rigid3d::Identity()}); + } + // Corkscrew translation and constant velocity rotation. + for (double t = 0.; t <= 0.6; t += 0.006) { + current_time += common::FromSeconds(0.3); + trajectory.push_back(TrajectoryNode{ + current_time, + transform::Rigid3d::Translation(Eigen::Vector3d( + std::sin(4. * t), 1. - std::cos(4. * t), 1. * t)) * + transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 0.3 * t, Eigen::Vector3d(1., -1., 2.).normalized()))}); + } + return trajectory; + } + + void VerifyAccuracy(const std::vector& expected_trajectory, + double expected_accuracy) { + int num_poses = 0; + for (const TrajectoryNode& node : expected_trajectory) { + AddLinearOnlyImuObservation(node.time, node.pose); + if (local_trajectory_builder_->AddLaserFan3D( + node.time, GenerateLaserFan(node.pose)) != nullptr) { + const auto pose_estimate = local_trajectory_builder_->pose_estimate(); + EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); + ++num_poses; + LOG(INFO) << "num_poses: " << num_poses; + } + } + } + + std::unique_ptr local_trajectory_builder_; + std::vector bubbles_; +}; + +TEST_F(KalmanLocalTrajectoryBuilderTest, + MoveInsideCubeUsingOnlyCeresScanMatcher) { + local_trajectory_builder_.reset( + new KalmanLocalTrajectoryBuilder(CreateTrajectoryBuilderOptions())); + VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1); +} + +} // namespace +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/laser_fan_inserter.cc b/cartographer/cartographer/mapping_3d/laser_fan_inserter.cc new file mode 100644 index 0000000..979b358 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/laser_fan_inserter.cc @@ -0,0 +1,95 @@ +/* + * 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_3d/laser_fan_inserter.h" + +#include "Eigen/Core" +#include "cartographer/mapping/probability_values.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +namespace { + +void InsertLaserMissesIntoGrid(const std::vector& miss_table, + const Eigen::Vector3f& origin, + const sensor::PointCloud& laser_returns, + HybridGrid* hybrid_grid, + const int num_free_space_voxels) { + const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin); + for (const Eigen::Vector3f& hit : laser_returns) { + const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); + + const Eigen::Array3i delta = hit_cell - origin_cell; + const int num_samples = delta.cwiseAbs().maxCoeff(); + CHECK_LT(num_samples, 1 << 15); + // 'num_samples' is the number of samples we equi-distantly place on the + // line between 'origin' and 'hit'. (including a fractional part for sub- + // voxels) It is chosen so that between two samples we change from one voxel + // to the next on the fastest changing dimension. + // + // Only the last 'num_free_space_voxels' are updated for performance. + for (int position = std::max(0, num_samples - num_free_space_voxels); + position < num_samples; ++position) { + const Eigen::Array3i miss_cell = + origin_cell + delta * position / num_samples; + hybrid_grid->ApplyLookupTable(miss_cell, miss_table); + } + } +} + +} // namespace + +proto::LaserFanInserterOptions CreateLaserFanInserterOptions( + common::LuaParameterDictionary* 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_num_free_space_voxels( + parameter_dictionary->GetInt("num_free_space_voxels")); + 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::LaserFan3D& laser_fan, + HybridGrid* hybrid_grid) const { + CHECK_NOTNULL(hybrid_grid)->StartUpdate(); + + for (const Eigen::Vector3f& hit : laser_fan.returns) { + const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); + hybrid_grid->ApplyLookupTable(hit_cell, hit_table_); + } + + // 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). + InsertLaserMissesIntoGrid(miss_table_, laser_fan.origin, laser_fan.returns, + hybrid_grid, options_.num_free_space_voxels()); +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/laser_fan_inserter.h b/cartographer/cartographer/mapping_3d/laser_fan_inserter.h new file mode 100644 index 0000000..05736e0 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/laser_fan_inserter.h @@ -0,0 +1,51 @@ +/* + * 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_3D_LASER_FAN_INSERTER_H_ +#define CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ + +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/proto/laser_fan_inserter_options.pb.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping_3d { + +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 'hybrid_grid'. + void Insert(const sensor::LaserFan3D& laser_fan, + HybridGrid* hybrid_grid) const; + + private: + const proto::LaserFanInserterOptions options_; + const std::vector hit_table_; + const std::vector miss_table_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ diff --git a/cartographer/cartographer/mapping_3d/laser_fan_inserter_test.cc b/cartographer/cartographer/mapping_3d/laser_fan_inserter_test.cc new file mode 100644 index 0000000..a2cbfcb --- /dev/null +++ b/cartographer/cartographer/mapping_3d/laser_fan_inserter_test.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/mapping_3d/laser_fan_inserter.h" + +#include +#include + +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_3d { +namespace { + +class LaserFanInserterTest : public ::testing::Test { + protected: + LaserFanInserterTest() + : hybrid_grid_(1.f, Eigen::Vector3f(0.5f, 0.5f, 0.5f)) { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "num_free_space_voxels = 1000, " + "}"); + options_ = CreateLaserFanInserterOptions(parameter_dictionary.get()); + laser_fan_inserter_.reset(new LaserFanInserter(options_)); + } + + void InsertPointCloud() { + const Eigen::Vector3f origin = Eigen::Vector3f(0.5f, 0.5f, -3.5f); + sensor::PointCloud laser_returns = {{-2.5f, -0.5f, 4.5f}, + {-1.5f, 0.5f, 4.5f}, + {-0.5f, 1.5f, 4.5f}, + {0.5f, 2.5f, 4.5f}}; + laser_fan_inserter_->Insert(sensor::LaserFan3D{origin, laser_returns, {}}, + &hybrid_grid_); + } + + float GetProbability(float x, float y, float z) const { + return hybrid_grid_.GetProbability( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + + float IsKnown(float x, float y, float z) const { + return hybrid_grid_.IsKnown( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + + const proto::LaserFanInserterOptions& options() const { return options_; } + + private: + HybridGrid hybrid_grid_; + std::unique_ptr laser_fan_inserter_; + proto::LaserFanInserterOptions options_; +}; + +TEST_F(LaserFanInserterTest, InsertPointCloud) { + InsertPointCloud(); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -3.5f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -2.5f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -1.5f), + 1e-4); + for (int x = -4; x <= 4; ++x) { + for (int y = -4; y <= 4; ++y) { + if (x < -3 || x > 0 || y != x + 2) { + EXPECT_FALSE(IsKnown(x + 0.5f, y + 0.5f, 4.5f)); + } else { + EXPECT_NEAR(options().hit_probability(), + GetProbability(x + 0.5f, y + 0.5f, 4.5f), 1e-4); + } + } + } +} + +TEST_F(LaserFanInserterTest, ProbabilityProgression) { + InsertPointCloud(); + EXPECT_NEAR(options().hit_probability(), GetProbability(-1.5f, 0.5f, 4.5f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(-1.5f, 0.5f, 3.5f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -2.5f), + 1e-4); + + for (int i = 0; i < 1000; ++i) { + InsertPointCloud(); + } + EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-1.5f, 0.5f, 4.5f), + 1e-3); + EXPECT_NEAR(mapping::kMinProbability, GetProbability(-1.5f, 0.5f, 3.5f), + 1e-3); + EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.5f, 0.5f, -2.5f), + 1e-3); +} + +} // namespace +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/cartographer/mapping_3d/local_trajectory_builder.cc new file mode 100644 index 0000000..8214434 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/local_trajectory_builder.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/mapping_3d/local_trajectory_builder.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" +#include "cartographer/mapping_3d/optimizing_local_trajectory_builder.h" + +namespace cartographer { +namespace mapping_3d { + +std::unique_ptr CreateLocalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& + local_trajectory_builder_options) { + switch (local_trajectory_builder_options.use()) { + case proto::LocalTrajectoryBuilderOptions::KALMAN: + return common::make_unique( + local_trajectory_builder_options); + case proto::LocalTrajectoryBuilderOptions::OPTIMIZING: + return common::make_unique( + local_trajectory_builder_options); + } + LOG(FATAL); +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/cartographer/mapping_3d/local_trajectory_builder.h new file mode 100644 index 0000000..09ca7ec --- /dev/null +++ b/cartographer/cartographer/mapping_3d/local_trajectory_builder.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_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ + +#include +#include + +#include "cartographer/mapping_3d/local_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" + +namespace cartographer { +namespace mapping_3d { + +std::unique_ptr CreateLocalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& + local_trajectory_builder_options); + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/cartographer/mapping_3d/local_trajectory_builder_interface.h new file mode 100644 index 0000000..ec4d6b7 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping_3d { + +class LocalTrajectoryBuilderInterface { + public: + using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; + + struct InsertionResult { + common::Time time; + sensor::LaserFan3D laser_fan_in_tracking; + transform::Rigid3d pose_observation; + kalman_filter::PoseCovariance covariance_estimate; + const Submaps* submaps; + const Submap* matching_submap; + std::vector insertion_submaps; + }; + + virtual ~LocalTrajectoryBuilderInterface() {} + + LocalTrajectoryBuilderInterface(const LocalTrajectoryBuilderInterface&) = + delete; + LocalTrajectoryBuilderInterface& operator=( + const LocalTrajectoryBuilderInterface&) = delete; + + virtual void AddImuData(common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) = 0; + virtual std::unique_ptr 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; + + // Register a 'trajectory_node_index' from the SparsePoseGraph corresponding + // to the latest inserted laser scan. This is used to remember which + // trajectory node should be used to visualize a Submap. + virtual void AddTrajectoryNodeIndex(int trajectory_node_index) = 0; + virtual mapping_3d::Submaps* submaps() = 0; + virtual const PoseEstimate& pose_estimate() const = 0; + virtual kalman_filter::PoseTracker* pose_tracker() const = 0; + + protected: + LocalTrajectoryBuilderInterface() {} +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ diff --git a/cartographer/cartographer/mapping_3d/local_trajectory_builder_options.cc b/cartographer/cartographer/mapping_3d/local_trajectory_builder_options.cc new file mode 100644 index 0000000..40fd0a8 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/local_trajectory_builder_options.cc @@ -0,0 +1,76 @@ +/* + * 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_3d/local_trajectory_builder_options.h" + +#include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h" +#include "cartographer/mapping_3d/motion_filter.h" +#include "cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/voxel_filter.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::LocalTrajectoryBuilderOptions options; + options.set_laser_min_range( + parameter_dictionary->GetDouble("laser_min_range")); + options.set_laser_max_range( + parameter_dictionary->GetDouble("laser_max_range")); + options.set_scans_per_accumulation( + parameter_dictionary->GetInt("scans_per_accumulation")); + options.set_laser_voxel_filter_size( + parameter_dictionary->GetDouble("laser_voxel_filter_size")); + *options.mutable_high_resolution_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("high_resolution_adaptive_voxel_filter") + .get()); + *options.mutable_low_resolution_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("low_resolution_adaptive_voxel_filter") + .get()); + *options.mutable_ceres_scan_matcher_options() = + scan_matching::CreateCeresScanMatcherOptions( + parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); + *options.mutable_motion_filter_options() = CreateMotionFilterOptions( + parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions( + parameter_dictionary->GetDictionary("submaps").get()); + *options.mutable_kalman_local_trajectory_builder_options() = + CreateKalmanLocalTrajectoryBuilderOptions( + parameter_dictionary->GetDictionary("kalman_local_trajectory_builder") + .get()); + *options.mutable_optimizing_local_trajectory_builder_options() = + CreateOptimizingLocalTrajectoryBuilderOptions( + parameter_dictionary + ->GetDictionary("optimizing_local_trajectory_builder") + .get()); + const string use_string = parameter_dictionary->GetString("use"); + proto::LocalTrajectoryBuilderOptions::Use use; + CHECK(proto::LocalTrajectoryBuilderOptions::Use_Parse(use_string, &use)) + << "Unknown local_trajectory_builder kind: " << use_string; + options.set_use(use); + return options; +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/local_trajectory_builder_options.h b/cartographer/cartographer/mapping_3d/local_trajectory_builder_options.h new file mode 100644 index 0000000..96d6508 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/local_trajectory_builder_options.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" + +namespace cartographer { +namespace mapping_3d { + +proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ diff --git a/cartographer/cartographer/mapping_3d/motion_filter.cc b/cartographer/cartographer/mapping_3d/motion_filter.cc new file mode 100644 index 0000000..cefcddf --- /dev/null +++ b/cartographer/cartographer/mapping_3d/motion_filter.cc @@ -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. + */ + +#include "cartographer/mapping_3d/motion_filter.h" + +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +proto::MotionFilterOptions CreateMotionFilterOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::MotionFilterOptions options; + options.set_max_time_seconds( + parameter_dictionary->GetDouble("max_time_seconds")); + options.set_max_distance_meters( + parameter_dictionary->GetDouble("max_distance_meters")); + options.set_max_angle_radians( + parameter_dictionary->GetDouble("max_angle_radians")); + return options; +} + +MotionFilter::MotionFilter(const proto::MotionFilterOptions& options) + : options_(options) {} + +bool MotionFilter::IsSimilar(const common::Time time, + const transform::Rigid3d& pose) { + LOG_EVERY_N(INFO, 500) << "Motion filter reduced the number of scans to " + << 100. * num_different_ / num_total_ << "%."; + ++num_total_; + if (num_total_ > 1 && + time - last_time_ <= common::FromSeconds(options_.max_time_seconds()) && + (pose.translation() - last_pose_.translation()).norm() <= + options_.max_distance_meters() && + transform::GetAngle(pose.inverse() * last_pose_) <= + options_.max_angle_radians()) { + return true; + } + last_time_ = time; + last_pose_ = pose; + ++num_different_; + return false; +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/motion_filter.h b/cartographer/cartographer/mapping_3d/motion_filter.h new file mode 100644 index 0000000..4b8327a --- /dev/null +++ b/cartographer/cartographer/mapping_3d/motion_filter.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ +#define CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping_3d/proto/motion_filter_options.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping_3d { + +proto::MotionFilterOptions CreateMotionFilterOptions( + common::LuaParameterDictionary* parameter_dictionary); + +// Takes poses as input and filters them to get fewer poses. +class MotionFilter { + public: + explicit MotionFilter(const proto::MotionFilterOptions& options); + + // If the accumulated motion (linear, rotational, or time) is above the + // threshold, returns false. Otherwise the relative motion is accumulated and + // true is returned. + bool IsSimilar(common::Time time, const transform::Rigid3d& pose); + + private: + const proto::MotionFilterOptions options_; + int num_total_ = 0; + int num_different_ = 0; + common::Time last_time_; + transform::Rigid3d last_pose_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ diff --git a/cartographer/cartographer/mapping_3d/motion_filter_test.cc b/cartographer/cartographer/mapping_3d/motion_filter_test.cc new file mode 100644 index 0000000..bfe9eb4 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/motion_filter_test.cc @@ -0,0 +1,109 @@ +/* + * 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_3d/motion_filter.h" + +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_3d { +namespace { + +class MotionFilterTest : public ::testing::Test { + protected: + MotionFilterTest() { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "max_time_seconds = 0.5, " + "max_distance_meters = 0.2, " + "max_angle_radians = 2., " + "}"); + options_ = CreateMotionFilterOptions(parameter_dictionary.get()); + } + + common::Time SecondsSinceEpoch(int seconds) { + return common::FromUniversal(seconds * 10000000); + } + + proto::MotionFilterOptions options_; +}; + +TEST_F(MotionFilterTest, NotInitialized) { + MotionFilter motion_filter(options_); + EXPECT_FALSE( + motion_filter.IsSimilar(common::Time(), transform::Rigid3d::Identity())); +} + +TEST_F(MotionFilterTest, NoChange) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); +} + +TEST_F(MotionFilterTest, TimeElapsed) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(43), + transform::Rigid3d::Identity())); + EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(43), + transform::Rigid3d::Identity())); +} + +TEST_F(MotionFilterTest, LinearMotion) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0., 0.)))); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.45, 0., 0.)))); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0., 0.)))); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), + transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0.15, 0.)))); +} + +TEST_F(MotionFilterTest, RotationalMotion) { + MotionFilter motion_filter(options_); + EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 1.9, Eigen::Vector3d::UnitY())))); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 2.1, Eigen::Vector3d::UnitY())))); + EXPECT_TRUE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 4., Eigen::Vector3d::UnitY())))); + EXPECT_FALSE(motion_filter.IsSimilar( + SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( + 5.9, Eigen::Vector3d::UnitY())))); + EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42), + transform::Rigid3d::Identity())); +} + +} // namespace +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc new file mode 100644 index 0000000..cc4c43f --- /dev/null +++ b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -0,0 +1,451 @@ +/* + * 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_3d/optimizing_local_trajectory_builder.h" + +#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/rotation_cost_function.h" +#include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" +#include "cartographer/mapping_3d/translation_cost_function.h" +#include "cartographer/transform/transform.h" +#include "cartographer/transform/transform_interpolation_buffer.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +namespace { + +// Computes the cost of differences between two velocities. For the constant +// velocity model the residuals are just the vector difference. +class VelocityDeltaCostFunctor { + public: + explicit VelocityDeltaCostFunctor(const double scaling_factor) + : scaling_factor_(scaling_factor) {} + + VelocityDeltaCostFunctor(const VelocityDeltaCostFunctor&) = delete; + VelocityDeltaCostFunctor& operator=(const VelocityDeltaCostFunctor&) = delete; + + template + bool operator()(const T* const old_velocity, const T* const new_velocity, + T* residual) const { + residual[0] = scaling_factor_ * (new_velocity[0] - old_velocity[0]); + residual[1] = scaling_factor_ * (new_velocity[1] - old_velocity[1]); + residual[2] = scaling_factor_ * (new_velocity[2] - old_velocity[2]); + return true; + } + + private: + const double scaling_factor_; +}; + +class RelativeTranslationAndYawCostFunction { + public: + RelativeTranslationAndYawCostFunction(const double translation_scaling_factor, + const double rotation_scaling_factor, + const transform::Rigid3d& delta) + : translation_scaling_factor_(translation_scaling_factor), + rotation_scaling_factor_(rotation_scaling_factor), + delta_(delta) {} + + RelativeTranslationAndYawCostFunction( + const RelativeTranslationAndYawCostFunction&) = delete; + RelativeTranslationAndYawCostFunction& operator=( + const RelativeTranslationAndYawCostFunction&) = delete; + + template + bool operator()(const T* const start_translation, + const T* const start_rotation, const T* const end_translation, + const T* const end_rotation, T* residual) const { + const transform::Rigid3 start( + Eigen::Map>(start_translation), + Eigen::Quaternion(start_rotation[0], start_rotation[1], + start_rotation[2], start_rotation[3])); + const transform::Rigid3 end( + Eigen::Map>(end_translation), + Eigen::Quaternion(end_rotation[0], end_rotation[1], end_rotation[2], + end_rotation[3])); + + const transform::Rigid3 delta = end.inverse() * start; + const transform::Rigid3 error = delta.inverse() * delta_.cast(); + residual[0] = translation_scaling_factor_ * error.translation().x(); + residual[1] = translation_scaling_factor_ * error.translation().y(); + residual[2] = translation_scaling_factor_ * error.translation().z(); + residual[3] = rotation_scaling_factor_ * transform::GetYaw(error); + return true; + } + + private: + const double translation_scaling_factor_; + const double rotation_scaling_factor_; + const transform::Rigid3d delta_; +}; + +} // namespace + +OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& options) + : options_(options), + ceres_solver_options_(common::CreateCeresSolverOptions( + options.ceres_scan_matcher_options().ceres_solver_options())), + submaps_(common::make_unique(options.submaps_options())), + num_accumulated_(0), + motion_filter_(options.motion_filter_options()) {} + +OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {} + +mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() { + return submaps_.get(); +} + +void OptimizingLocalTrajectoryBuilder::AddImuData( + const common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) { + imu_data_.push_back(ImuData{ + time, linear_acceleration, angular_velocity, + }); + RemoveObsoleteSensorData(); +} + +void OptimizingLocalTrajectoryBuilder::AddOdometerPose( + const common::Time time, const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) { + odometer_data_.push_back(OdometerData{time, pose}); + RemoveObsoleteSensorData(); +} + +std::unique_ptr +OptimizingLocalTrajectoryBuilder::AddLaserFan3D( + const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking) { + CHECK_GT(laser_fan_in_tracking.returns.size(), 0); + + // TODO(hrapp): Handle misses. + // TODO(hrapp): Where are NaNs in laser_fan_in_tracking coming from? + sensor::PointCloud point_cloud; + for (const Eigen::Vector3f& laser_return : laser_fan_in_tracking.returns) { + const Eigen::Vector3f delta = laser_return - laser_fan_in_tracking.origin; + const float range = delta.norm(); + if (range >= options_.laser_min_range()) { + if (range <= options_.laser_max_range()) { + point_cloud.push_back(laser_return); + } + } + } + + auto high_resolution_options = + options_.high_resolution_adaptive_voxel_filter_options(); + high_resolution_options.set_min_num_points( + high_resolution_options.min_num_points() / + options_.scans_per_accumulation()); + sensor::AdaptiveVoxelFilter high_resolution_adaptive_voxel_filter( + high_resolution_options); + const sensor::PointCloud high_resolution_filtered_points = + high_resolution_adaptive_voxel_filter.Filter(point_cloud); + + auto low_resolution_options = + options_.low_resolution_adaptive_voxel_filter_options(); + low_resolution_options.set_min_num_points( + low_resolution_options.min_num_points() / + options_.scans_per_accumulation()); + sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( + low_resolution_options); + const sensor::PointCloud low_resolution_filtered_points = + low_resolution_adaptive_voxel_filter.Filter(point_cloud); + + if (batches_.empty()) { + // First laser ever. Initialize to the origin. + batches_.push_back( + Batch{time, point_cloud, high_resolution_filtered_points, + low_resolution_filtered_points, + State{{{1., 0., 0., 0.}}, {{0., 0., 0.}}, {{0., 0., 0.}}}}); + } else { + const Batch& last_batch = batches_.back(); + batches_.push_back(Batch{ + time, point_cloud, high_resolution_filtered_points, + low_resolution_filtered_points, + PredictState(last_batch.state, last_batch.time, time), + }); + } + ++num_accumulated_; + + RemoveObsoleteSensorData(); + return MaybeOptimize(time); +} + +void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() { + if (imu_data_.empty()) { + batches_.clear(); + return; + } + + while (batches_.size() > + static_cast(options_.scans_per_accumulation())) { + batches_.pop_front(); + } + + while (imu_data_.size() > 1 && + (batches_.empty() || imu_data_[1].time <= batches_.front().time)) { + imu_data_.pop_front(); + } + + while ( + odometer_data_.size() > 1 && + (batches_.empty() || odometer_data_[1].time <= batches_.front().time)) { + odometer_data_.pop_front(); + } +} + +std::unique_ptr +OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { + // TODO(hrapp): Make the number of optimizations configurable. + if (num_accumulated_ < options_.scans_per_accumulation() && + num_accumulated_ % 10 != 0) { + return nullptr; + } + + ceres::Problem problem; + for (size_t i = 0; i < batches_.size(); ++i) { + Batch& batch = batches_[i]; + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new scan_matching::OccupiedSpaceCostFunctor( + options_.optimizing_local_trajectory_builder_options() + .high_resolution_grid_scale() / + std::sqrt(static_cast( + batch.high_resolution_filtered_points.size())), + batch.high_resolution_filtered_points, + submaps_->high_resolution_matching_grid()), + batch.high_resolution_filtered_points.size()), + nullptr, batch.state.translation.data(), batch.state.rotation.data()); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new scan_matching::OccupiedSpaceCostFunctor( + options_.optimizing_local_trajectory_builder_options() + .low_resolution_grid_scale() / + std::sqrt(static_cast( + batch.low_resolution_filtered_points.size())), + batch.low_resolution_filtered_points, + submaps_->low_resolution_matching_grid()), + batch.low_resolution_filtered_points.size()), + nullptr, batch.state.translation.data(), batch.state.rotation.data()); + + if (i == 0) { + problem.SetParameterBlockConstant(batch.state.translation.data()); + problem.AddParameterBlock(batch.state.velocity.data(), 3); + problem.SetParameterBlockConstant(batch.state.velocity.data()); + problem.SetParameterBlockConstant(batch.state.rotation.data()); + } else { + problem.SetParameterization(batch.state.rotation.data(), + new ceres::QuaternionParameterization()); + } + } + + auto it = imu_data_.cbegin(); + for (size_t i = 1; i < batches_.size(); ++i) { + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new VelocityDeltaCostFunctor( + options_.optimizing_local_trajectory_builder_options() + .velocity_scale())), + nullptr, batches_[i - 1].state.velocity.data(), + batches_[i].state.velocity.data()); + + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new TranslationCostFunction( + options_.optimizing_local_trajectory_builder_options() + .translation_scale(), + common::ToSeconds(batches_[i].time - batches_[i - 1].time))), + nullptr, batches_[i - 1].state.translation.data(), + batches_[i].state.translation.data(), + batches_[i - 1].state.velocity.data()); + + const IntegrateImuResult result = + IntegrateImu(imu_data_, batches_[i - 1].time, batches_[i].time, &it); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new RotationCostFunction( + options_.optimizing_local_trajectory_builder_options() + .rotation_scale(), + result.delta_rotation)), + nullptr, batches_[i - 1].state.rotation.data(), + batches_[i].state.rotation.data()); + } + + if (odometer_data_.size() > 1) { + transform::TransformInterpolationBuffer interpolation_buffer; + for (const auto& odometer_data : odometer_data_) { + interpolation_buffer.Push(odometer_data.time, odometer_data.pose); + } + for (size_t i = 1; i < batches_.size(); ++i) { + // Only add constraints for this laser if we have bracketing data from + // the odometer. + if (!(interpolation_buffer.earliest_time() <= batches_[i - 1].time && + batches_[i].time <= interpolation_buffer.latest_time())) { + continue; + } + const transform::Rigid3d previous_odometer_pose = + interpolation_buffer.Lookup(batches_[i - 1].time); + const transform::Rigid3d current_odometer_pose = + interpolation_buffer.Lookup(batches_[i].time); + const transform::Rigid3d delta_pose = + current_odometer_pose.inverse() * previous_odometer_pose; + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new RelativeTranslationAndYawCostFunction( + options_.optimizing_local_trajectory_builder_options() + .odometry_translation_scale(), + options_.optimizing_local_trajectory_builder_options() + .odometry_rotation_scale(), + delta_pose)), + nullptr, batches_[i - 1].state.translation.data(), + batches_[i - 1].state.rotation.data(), + batches_[i].state.translation.data(), + batches_[i].state.rotation.data()); + } + } + + ceres::Solver::Summary summary; + ceres::Solve(ceres_solver_options_, &problem, &summary); + if (num_accumulated_ < options_.scans_per_accumulation()) { + return nullptr; + } + + num_accumulated_ = 0; + + const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid(); + sensor::LaserFan3D accumulated_laser_fan_in_tracking = { + Eigen::Vector3f::Zero(), {}, {}}; + + for (const auto& batch : batches_) { + const transform::Rigid3f transform = + (optimized_pose.inverse() * batch.state.ToRigid()).cast(); + for (const Eigen::Vector3f& point : batch.points) { + accumulated_laser_fan_in_tracking.returns.push_back(transform * point); + } + } + + return AddAccumulatedLaserFan3D(time, optimized_pose, + accumulated_laser_fan_in_tracking); +} + +std::unique_ptr +OptimizingLocalTrajectoryBuilder::AddAccumulatedLaserFan3D( + const common::Time time, const transform::Rigid3d& optimized_pose, + const sensor::LaserFan3D& laser_fan_in_tracking) { + const sensor::LaserFan3D filtered_laser_fan = { + laser_fan_in_tracking.origin, + sensor::VoxelFiltered(laser_fan_in_tracking.returns, + options_.laser_voxel_filter_size()), + sensor::VoxelFiltered(laser_fan_in_tracking.misses, + options_.laser_voxel_filter_size())}; + + if (filtered_laser_fan.returns.empty()) { + LOG(WARNING) << "Dropped empty laser scanner point cloud."; + return nullptr; + } + + const kalman_filter::PoseCovariance covariance = + 1e-7 * kalman_filter::PoseCovariance::Identity(); + + last_pose_estimate_ = { + time, + {optimized_pose, covariance}, + {optimized_pose, covariance}, + {optimized_pose, covariance}, + optimized_pose, + sensor::TransformPointCloud(filtered_laser_fan.returns, + optimized_pose.cast())}; + + return InsertIntoSubmap(time, filtered_laser_fan, optimized_pose, covariance); +} + +const OptimizingLocalTrajectoryBuilder::PoseEstimate& +OptimizingLocalTrajectoryBuilder::pose_estimate() const { + return last_pose_estimate_; +} + +void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex( + int trajectory_node_index) { + submaps_->AddTrajectoryNodeIndex(trajectory_node_index); +} + +std::unique_ptr +OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( + const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const transform::Rigid3d& pose_observation, + const kalman_filter::PoseCovariance& covariance_estimate) { + if (motion_filter_.IsSimilar(time, pose_observation)) { + return nullptr; + } + const Submap* const matching_submap = + submaps_->Get(submaps_->matching_index()); + std::vector insertion_submaps; + for (int insertion_index : submaps_->insertion_indices()) { + insertion_submaps.push_back(submaps_->Get(insertion_index)); + } + submaps_->InsertLaserFan(sensor::TransformLaserFan3D( + laser_fan_in_tracking, pose_observation.cast())); + return std::unique_ptr(new InsertionResult{ + time, laser_fan_in_tracking, pose_observation, covariance_estimate, + submaps_.get(), matching_submap, insertion_submaps}); +} + +OptimizingLocalTrajectoryBuilder::State +OptimizingLocalTrajectoryBuilder::PredictState(const State& start_state, + const common::Time start_time, + const common::Time end_time) { + auto it = --imu_data_.cend(); + while (it->time > start_time) { + CHECK(it != imu_data_.cbegin()); + --it; + } + + const IntegrateImuResult result = + IntegrateImu(imu_data_, start_time, end_time, &it); + + const Eigen::Quaterniond start_rotation( + start_state.rotation[0], start_state.rotation[1], start_state.rotation[2], + start_state.rotation[3]); + const Eigen::Quaterniond orientation = start_rotation * result.delta_rotation; + const double delta_time_seconds = common::ToSeconds(end_time - start_time); + + // TODO(hrapp): IntegrateImu should integration position as well. + const Eigen::Vector3d position = + Eigen::Map(start_state.translation.data()) + + delta_time_seconds * + Eigen::Map(start_state.velocity.data()); + const Eigen::Vector3d velocity = + Eigen::Map(start_state.velocity.data()) + + start_rotation * result.delta_velocity - + gravity_constant_ * delta_time_seconds * Eigen::Vector3d::UnitZ(); + + return State{ + {{orientation.w(), orientation.x(), orientation.y(), orientation.z()}}, + {{position.x(), position.y(), position.z()}}, + {{velocity.x(), velocity.y(), velocity.z()}}}; +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder.h new file mode 100644 index 0000000..28411d0 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -0,0 +1,138 @@ +/* + * 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_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_ + +#include +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/imu_integration.h" +#include "cartographer/mapping_3d/local_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/motion_filter.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/voxel_filter.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping_3d { + +// Batches up some sensor data and optimizes them in one go to get a locally +// consistent trajectory. +class OptimizingLocalTrajectoryBuilder + : public LocalTrajectoryBuilderInterface { + public: + explicit OptimizingLocalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& options); + ~OptimizingLocalTrajectoryBuilder() override; + + OptimizingLocalTrajectoryBuilder(const OptimizingLocalTrajectoryBuilder&) = + delete; + OptimizingLocalTrajectoryBuilder& operator=( + const OptimizingLocalTrajectoryBuilder&) = delete; + + void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) override; + std::unique_ptr AddLaserFan3D( + common::Time time, + const sensor::LaserFan3D& laser_fan_in_tracking) override; + + void AddOdometerPose( + const common::Time time, const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) override; + + kalman_filter::PoseTracker* pose_tracker() const override { return nullptr; } + + void AddTrajectoryNodeIndex(int trajectory_node_index) override; + mapping_3d::Submaps* submaps() override; + const PoseEstimate& pose_estimate() const override; + + private: + struct State { + // TODO(hrapp): This should maybe use a CeresPose. + // Rotation quaternion as (w, x, y, z). + std::array rotation; + std::array translation; + std::array velocity; + + Eigen::Quaterniond ToQuaternion() const { + return Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], + rotation[3]); + } + + transform::Rigid3d ToRigid() const { + return transform::Rigid3d( + Eigen::Vector3d(translation[0], translation[1], translation[2]), + ToQuaternion()); + } + }; + + struct Batch { + common::Time time; + sensor::PointCloud points; + sensor::PointCloud high_resolution_filtered_points; + sensor::PointCloud low_resolution_filtered_points; + State state; + }; + + struct OdometerData { + common::Time time; + + // Dead-reckoning pose of the odometry. + transform::Rigid3d pose; + }; + + State PredictState(const State& start_state, const common::Time start_time, + const common::Time end_time); + + void RemoveObsoleteSensorData(); + + std::unique_ptr AddAccumulatedLaserFan3D( + common::Time time, const transform::Rigid3d& pose_observation, + const sensor::LaserFan3D& laser_fan_in_tracking); + + std::unique_ptr InsertIntoSubmap( + const common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const transform::Rigid3d& pose_observation, + const kalman_filter::PoseCovariance& covariance_estimate); + + std::unique_ptr MaybeOptimize(common::Time time); + + const proto::LocalTrajectoryBuilderOptions options_; + const ceres::Solver::Options ceres_solver_options_; + std::unique_ptr submaps_; + int num_accumulated_; + + std::deque batches_; + double gravity_constant_ = 9.8; + std::deque imu_data_; + std::deque odometer_data_; + + PoseEstimate last_pose_estimate_; + + MotionFilter motion_filter_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc new file mode 100644 index 0000000..0390d8e --- /dev/null +++ b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.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_3d/optimizing_local_trajectory_builder_options.h" + +namespace cartographer { +namespace mapping_3d { + +proto::OptimizingLocalTrajectoryBuilderOptions +CreateOptimizingLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::OptimizingLocalTrajectoryBuilderOptions options; + options.set_high_resolution_grid_scale( + parameter_dictionary->GetDouble("high_resolution_grid_scale")); + options.set_low_resolution_grid_scale( + parameter_dictionary->GetDouble("low_resolution_grid_scale")); + options.set_velocity_scale(parameter_dictionary->GetDouble("velocity_scale")); + options.set_translation_scale( + parameter_dictionary->GetDouble("translation_scale")); + options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale")); + options.set_odometry_translation_scale( + parameter_dictionary->GetDouble("odometry_translation_scale")); + options.set_odometry_rotation_scale( + parameter_dictionary->GetDouble("odometry_rotation_scale")); + return options; +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h new file mode 100644 index 0000000..5580759 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h @@ -0,0 +1,33 @@ +/* + * 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_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h" + +namespace cartographer { +namespace mapping_3d { + +proto::OptimizingLocalTrajectoryBuilderOptions +CreateOptimizingLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ diff --git a/cartographer/cartographer/mapping_3d/proto/CMakeLists.txt b/cartographer/cartographer/mapping_3d/proto/CMakeLists.txt new file mode 100644 index 0000000..f16928c --- /dev/null +++ b/cartographer/cartographer/mapping_3d/proto/CMakeLists.txt @@ -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. + +google_proto_library(mapping_3d_proto_kalman_local_trajectory_builder_options + SRCS + kalman_local_trajectory_builder_options.proto + DEPENDS + kalman_filter_proto_pose_tracker_options + mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options +) + +google_proto_library(mapping_3d_proto_laser_fan_inserter_options + SRCS + laser_fan_inserter_options.proto +) + +google_proto_library(mapping_3d_proto_local_trajectory_builder_options + SRCS + local_trajectory_builder_options.proto + DEPENDS + mapping_3d_proto_kalman_local_trajectory_builder_options + mapping_3d_proto_motion_filter_options + mapping_3d_proto_optimizing_local_trajectory_builder_options + mapping_3d_proto_submaps_options + mapping_3d_scan_matching_proto_ceres_scan_matcher_options + sensor_proto_adaptive_voxel_filter_options +) + +google_proto_library(mapping_3d_proto_motion_filter_options + SRCS + motion_filter_options.proto +) + +google_proto_library(mapping_3d_proto_optimizing_local_trajectory_builder_options + SRCS + optimizing_local_trajectory_builder_options.proto +) + +google_proto_library(mapping_3d_proto_submaps_options + SRCS + submaps_options.proto + DEPENDS + mapping_3d_proto_laser_fan_inserter_options +) diff --git a/cartographer/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto b/cartographer/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto new file mode 100644 index 0000000..a5eb823 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.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"; + +import "cartographer/kalman_filter/proto/pose_tracker_options.proto"; +import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; + +package cartographer.mapping_3d.proto; + +message KalmanLocalTrajectoryBuilderOptions { + // 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 = 1; + + optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions + real_time_correlative_scan_matcher_options = 2; + optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3; +} diff --git a/cartographer/cartographer/mapping_3d/proto/laser_fan_inserter_options.proto b/cartographer/cartographer/mapping_3d/proto/laser_fan_inserter_options.proto new file mode 100644 index 0000000..f03fddb --- /dev/null +++ b/cartographer/cartographer/mapping_3d/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_3d.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; + + // Up to how many free space voxels are updated for scan matching. + // 0 disables free space. + optional int32 num_free_space_voxels = 3; +} diff --git a/cartographer/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto b/cartographer/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto new file mode 100644 index 0000000..da02469 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto @@ -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. + +syntax = "proto2"; + +import "cartographer/mapping_3d/proto/motion_filter_options.proto"; +import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; +import "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto"; +import "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto"; +import "cartographer/mapping_3d/proto/submaps_options.proto"; +import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; + +package cartographer.mapping_3d.proto; + +message LocalTrajectoryBuilderOptions { + enum Use { + KALMAN = 0; + OPTIMIZING = 1; + } + + // Laser limits. + optional float laser_min_range = 1; + optional float laser_max_range = 2; + + // Number of scans to accumulate into one unwarped, combined scan to use for + // scan matching. + optional int32 scans_per_accumulation = 3; + + // Voxel filter that gets applied to the laser before doing anything with it. + optional float laser_voxel_filter_size = 4; + + // Voxel filter used to compute a sparser point cloud for matching. + optional sensor.proto.AdaptiveVoxelFilterOptions + high_resolution_adaptive_voxel_filter_options = 5; + optional sensor.proto.AdaptiveVoxelFilterOptions + low_resolution_adaptive_voxel_filter_options = 12; + + optional scan_matching.proto.CeresScanMatcherOptions + ceres_scan_matcher_options = 6; + optional MotionFilterOptions motion_filter_options = 7; + optional SubmapsOptions submaps_options = 8; + + // Which one of the implementation to instantiate and use. + optional Use use = 9; + optional KalmanLocalTrajectoryBuilderOptions + kalman_local_trajectory_builder_options = 10; + optional OptimizingLocalTrajectoryBuilderOptions + optimizing_local_trajectory_builder_options = 11; +} diff --git a/cartographer/cartographer/mapping_3d/proto/motion_filter_options.proto b/cartographer/cartographer/mapping_3d/proto/motion_filter_options.proto new file mode 100644 index 0000000..539c19e --- /dev/null +++ b/cartographer/cartographer/mapping_3d/proto/motion_filter_options.proto @@ -0,0 +1,28 @@ +// 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_3d.proto; + +message MotionFilterOptions { + // Threshold above which a new scan is inserted based on time. + optional double max_time_seconds = 1; + + // Threshold above which a new scan is inserted based on linear motion. + optional double max_distance_meters = 2; + + // Threshold above which a new scan is inserted based on rotational motion. + optional double max_angle_radians = 3; +} diff --git a/cartographer/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto b/cartographer/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto new file mode 100644 index 0000000..ba5cded --- /dev/null +++ b/cartographer/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto @@ -0,0 +1,27 @@ +// 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_3d.proto; + +message OptimizingLocalTrajectoryBuilderOptions { + optional double high_resolution_grid_scale = 6; + optional double low_resolution_grid_scale = 7; + optional double velocity_scale = 1; + optional double translation_scale = 2; + optional double rotation_scale = 3; + optional double odometry_translation_scale = 4; + optional double odometry_rotation_scale = 5; +} diff --git a/cartographer/cartographer/mapping_3d/proto/submaps_options.proto b/cartographer/cartographer/mapping_3d/proto/submaps_options.proto new file mode 100644 index 0000000..2353a9d --- /dev/null +++ b/cartographer/cartographer/mapping_3d/proto/submaps_options.proto @@ -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. + +syntax = "proto2"; + +import "cartographer/mapping_3d/proto/laser_fan_inserter_options.proto"; + +package cartographer.mapping_3d.proto; + +message SubmapsOptions { + // Resolution of the 'high_resolution' map in meters used for local SLAM and + // loop closure. + optional double high_resolution = 1; + + // Maximum range to filter the point cloud to before insertion into the + // 'high_resolution' map. + optional double high_resolution_max_range = 4; + + // Resolution of the 'low_resolution' version of the map in meters used for + // local SLAM only. + optional double low_resolution = 5; + + // 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 = 2; + + optional LaserFanInserterOptions laser_fan_inserter_options = 3; +} diff --git a/cartographer/cartographer/mapping_3d/rotation_cost_function.h b/cartographer/cartographer/mapping_3d/rotation_cost_function.h new file mode 100644 index 0000000..17abb02 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/rotation_cost_function.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_3D_ROTATION_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" + +namespace cartographer { +namespace mapping_3d { + +// Penalizes differences between IMU data and optimized orientations. +class RotationCostFunction { + public: + RotationCostFunction(const double scaling_factor, + const Eigen::Quaterniond& delta_rotation_imu_frame) + : scaling_factor_(scaling_factor), + delta_rotation_imu_frame_(delta_rotation_imu_frame) {} + + RotationCostFunction(const RotationCostFunction&) = delete; + RotationCostFunction& operator=(const RotationCostFunction&) = delete; + + template + bool operator()(const T* const start_rotation, const T* const end_rotation, + T* residual) const { + const Eigen::Quaternion start(start_rotation[0], start_rotation[1], + start_rotation[2], start_rotation[3]); + const Eigen::Quaternion end(end_rotation[0], end_rotation[1], + end_rotation[2], end_rotation[3]); + const Eigen::Quaternion error = + end.conjugate() * start * delta_rotation_imu_frame_.cast(); + residual[0] = scaling_factor_ * error.x(); + residual[1] = scaling_factor_ * error.y(); + residual[2] = scaling_factor_ * error.z(); + return true; + } + + private: + const double scaling_factor_; + const Eigen::Quaterniond delta_rotation_imu_frame_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/CMakeLists.txt b/cartographer/cartographer/mapping_3d/scan_matching/CMakeLists.txt new file mode 100644 index 0000000..b0d3534 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/CMakeLists.txt @@ -0,0 +1,192 @@ +# 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_3d_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 + common_make_unique + kalman_filter_pose_tracker + mapping_3d_ceres_pose + mapping_3d_hybrid_grid + mapping_3d_scan_matching_occupied_space_cost_functor + mapping_3d_scan_matching_proto_ceres_scan_matcher_options + mapping_3d_scan_matching_rotation_delta_cost_functor + mapping_3d_scan_matching_translation_delta_cost_functor + sensor_point_cloud + transform_rigid_transform + transform_transform +) + +google_library(mapping_3d_scan_matching_fast_correlative_scan_matcher + USES_CERES + USES_EIGEN + SRCS + fast_correlative_scan_matcher.cc + HDRS + fast_correlative_scan_matcher.h + DEPENDS + common_make_unique + common_math + common_port + mapping_2d_scan_matching_fast_correlative_scan_matcher + mapping_3d_hybrid_grid + mapping_3d_scan_matching_precomputation_grid + mapping_3d_scan_matching_proto_fast_correlative_scan_matcher_options + mapping_3d_scan_matching_rotational_scan_matcher + mapping_trajectory_node + sensor_point_cloud + transform_transform +) + +google_library(mapping_3d_scan_matching_interpolated_grid + HDRS + interpolated_grid.h + DEPENDS + mapping_3d_hybrid_grid +) + +google_library(mapping_3d_scan_matching_occupied_space_cost_functor + USES_EIGEN + HDRS + occupied_space_cost_functor.h + DEPENDS + mapping_3d_hybrid_grid + mapping_3d_scan_matching_interpolated_grid + sensor_point_cloud + transform_rigid_transform + transform_transform +) + +google_library(mapping_3d_scan_matching_precomputation_grid + USES_CERES + USES_EIGEN + SRCS + precomputation_grid.cc + HDRS + precomputation_grid.h + DEPENDS + common_math + mapping_3d_hybrid_grid + mapping_probability_values +) + +google_library(mapping_3d_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_math + mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options + mapping_3d_hybrid_grid + sensor_point_cloud + transform_transform +) + +google_library(mapping_3d_scan_matching_rotation_delta_cost_functor + USES_CERES + USES_EIGEN + HDRS + rotation_delta_cost_functor.h + DEPENDS + transform_rigid_transform + transform_transform +) + +google_library(mapping_3d_scan_matching_rotational_scan_matcher + USES_EIGEN + SRCS + rotational_scan_matcher.cc + HDRS + rotational_scan_matcher.h + DEPENDS + common_math + mapping_trajectory_node + sensor_point_cloud +) + +google_library(mapping_3d_scan_matching_translation_delta_cost_functor + USES_EIGEN + HDRS + translation_delta_cost_functor.h + DEPENDS + transform_rigid_transform +) + +google_test(mapping_3d_scan_matching_ceres_scan_matcher_test + USES_EIGEN + SRCS + ceres_scan_matcher_test.cc + DEPENDS + common_lua_parameter_dictionary_test_helpers + mapping_3d_hybrid_grid + mapping_3d_scan_matching_ceres_scan_matcher + sensor_point_cloud + transform_rigid_transform + transform_rigid_transform_test_helpers +) + +google_test(mapping_3d_scan_matching_fast_correlative_scan_matcher_test + SRCS + fast_correlative_scan_matcher_test.cc + DEPENDS + common_lua_parameter_dictionary_test_helpers + mapping_3d_laser_fan_inserter + mapping_3d_scan_matching_fast_correlative_scan_matcher + transform_rigid_transform_test_helpers + transform_transform +) + +google_test(mapping_3d_scan_matching_interpolated_grid_test + USES_EIGEN + SRCS + interpolated_grid_test.cc + DEPENDS + mapping_3d_hybrid_grid + mapping_3d_scan_matching_interpolated_grid +) + +google_test(mapping_3d_scan_matching_precomputation_grid_test + SRCS + precomputation_grid_test.cc + DEPENDS + mapping_3d_hybrid_grid + mapping_3d_scan_matching_precomputation_grid +) + +google_test(mapping_3d_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 + mapping_2d_scan_matching_real_time_correlative_scan_matcher + mapping_3d_hybrid_grid + mapping_3d_scan_matching_real_time_correlative_scan_matcher + sensor_point_cloud + transform_rigid_transform + transform_rigid_transform_test_helpers + transform_transform +) diff --git a/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc new file mode 100644 index 0000000..78e246f --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc @@ -0,0 +1,153 @@ +/* + * 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_3d/scan_matching/ceres_scan_matcher.h" + +#include +#include +#include + +#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/mapping_3d/ceres_pose.h" +#include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h" +#include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h" +#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { +namespace { + +struct YawOnlyQuaternionPlus { + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { + const T clamped_delta = common::Clamp(delta[0], T(-0.5), T(0.5)); + T q_delta[4]; + q_delta[0] = ceres::sqrt(1. - clamped_delta * clamped_delta); + q_delta[1] = T(0.); + q_delta[2] = T(0.); + q_delta[3] = clamped_delta; + ceres::QuaternionProduct(q_delta, x, x_plus_delta); + return true; + } +}; + +} // namespace + +proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::CeresScanMatcherOptions options; + for (int i = 0;; ++i) { + const string lua_identifier = + "occupied_space_cost_functor_weight_" + std::to_string(i); + if (!parameter_dictionary->HasKey(lua_identifier)) { + break; + } + options.add_occupied_space_cost_functor_weight( + parameter_dictionary->GetDouble(lua_identifier)); + } + options.set_previous_pose_translation_delta_cost_functor_weight( + parameter_dictionary->GetDouble( + "previous_pose_translation_delta_cost_functor_weight")); + options.set_initial_pose_estimate_rotation_delta_cost_functor_weight( + parameter_dictionary->GetDouble( + "initial_pose_estimate_rotation_delta_cost_functor_weight")); + options.set_covariance_scale( + parameter_dictionary->GetDouble("covariance_scale")); + options.set_only_optimize_yaw( + parameter_dictionary->GetBool("only_optimize_yaw")); + *options.mutable_ceres_solver_options() = + common::CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("ceres_solver_options").get()); + return options; +} + +CeresScanMatcher::CeresScanMatcher( + const proto::CeresScanMatcherOptions& options) + : options_(options), + ceres_solver_options_( + common::CreateCeresSolverOptions(options.ceres_solver_options())) { + ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; +} + +void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, + const transform::Rigid3d& initial_pose_estimate, + const std::vector& + point_clouds_and_hybrid_grids, + transform::Rigid3d* const pose_estimate, + kalman_filter::PoseCovariance* const covariance, + ceres::Solver::Summary* const summary) { + ceres::Problem problem; + CeresPose ceres_pose( + initial_pose_estimate, + options_.only_optimize_yaw() + ? std::unique_ptr( + common::make_unique>()) + : std::unique_ptr( + common::make_unique()), + &problem); + + CHECK_EQ(options_.occupied_space_cost_functor_weight_size(), + point_clouds_and_hybrid_grids.size()); + for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { + CHECK_GT(options_.occupied_space_cost_functor_weight(i), 0.); + const sensor::PointCloud& point_cloud = + *point_clouds_and_hybrid_grids[i].first; + const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new OccupiedSpaceCostFunctor( + options_.occupied_space_cost_functor_weight(i) / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, hybrid_grid), + point_cloud.size()), + nullptr, ceres_pose.translation(), ceres_pose.rotation()); + } + CHECK_GT(options_.previous_pose_translation_delta_cost_functor_weight(), 0.); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new TranslationDeltaCostFunctor( + options_.previous_pose_translation_delta_cost_functor_weight(), + previous_pose)), + nullptr, ceres_pose.translation()); + CHECK_GT(options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), + 0.); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction(new RotationDeltaCostFunctor( + options_.initial_pose_estimate_rotation_delta_cost_functor_weight(), + initial_pose_estimate.rotation())), + nullptr, ceres_pose.rotation()); + + ceres::Solve(ceres_solver_options_, &problem, summary); + + *pose_estimate = ceres_pose.ToRigid(); + + // TODO(whess): Remove once the UKF is gone. + *covariance = 1e-5 * options_.covariance_scale() * + kalman_filter::PoseCovariance::Identity(); +} + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h b/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h new file mode 100644 index 0000000..d12ea7f --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h @@ -0,0 +1,69 @@ +/* + * 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_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( + common::LuaParameterDictionary* parameter_dictionary); + +using PointCloudAndHybridGridPointers = + std::pair; + +// This scan matcher uses Ceres to align scans with an existing map. +class CeresScanMatcher { + public: + explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options); + + CeresScanMatcher(const CeresScanMatcher&) = delete; + CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; + + // Aligns 'point_clouds' within the 'hybrid_grids' given an + // 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and + // the solver 'summary'. + void Match(const transform::Rigid3d& previous_pose, + const transform::Rigid3d& initial_pose_estimate, + const std::vector& + point_clouds_and_hybrid_grids, + transform::Rigid3d* pose_estimate, + kalman_filter::PoseCovariance* covariance, + ceres::Solver::Summary* summary); + + private: + const proto::CeresScanMatcherOptions options_; + ceres::Solver::Options ceres_solver_options_; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc new file mode 100644 index 0000000..d2809d3 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc @@ -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. + */ + +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { +namespace { + +class CeresScanMatcherTest : public ::testing::Test { + protected: + CeresScanMatcherTest() + : hybrid_grid_(1.f /* resolution */, + Eigen::Vector3f(0.5f, 0.5f, 0.5f) /* origin */), + expected_pose_( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5))) { + for (const auto& point : + {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), + Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), + Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), + Eigen::Vector3f(-7.f, 3.f, 1.f)}) { + point_cloud_.push_back(point); + hybrid_grid_.SetProbability( + hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); + } + + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + occupied_space_cost_functor_weight_0 = 1., + previous_pose_translation_delta_cost_functor_weight = 0.01, + initial_pose_estimate_rotation_delta_cost_functor_weight = 0.1, + covariance_scale = 10., + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 10, + num_threads = 1, + }, + })text"); + options_ = CreateCeresScanMatcherOptions(parameter_dictionary.get()); + ceres_scan_matcher_.reset(new CeresScanMatcher(options_)); + } + + void TestFromInitialPose(const transform::Rigid3d& initial_pose) { + transform::Rigid3d pose; + kalman_filter::PoseCovariance covariance; + + ceres::Solver::Summary summary; + ceres_scan_matcher_->Match(initial_pose, initial_pose, + {{&point_cloud_, &hybrid_grid_}}, &pose, + &covariance, &summary); + EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); + EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); + } + + HybridGrid hybrid_grid_; + transform::Rigid3d expected_pose_; + sensor::PointCloud point_cloud_; + proto::CeresScanMatcherOptions options_; + std::unique_ptr ceres_scan_matcher_; +}; + +TEST_F(CeresScanMatcherTest, PerfectEstimate) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5))); +} + +TEST_F(CeresScanMatcherTest, AlongX) { + ceres_scan_matcher_.reset(new CeresScanMatcher(options_)); + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.3, 0.5, 0.5))); +} + +TEST_F(CeresScanMatcherTest, AlongZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.3))); +} + +TEST_F(CeresScanMatcherTest, AlongXYZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.4, 0.3, 0.7))); +} + +TEST_F(CeresScanMatcherTest, FullPoseCorrection) { + // We try to find the rotation around z... + const auto additional_transform = transform::Rigid3d::Rotation( + Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.))); + point_cloud_ = sensor::TransformPointCloud( + point_cloud_, additional_transform.cast()); + expected_pose_ = expected_pose_ * additional_transform.inverse(); + // ...starting initially with rotation around x. + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.45, 0.45, 0.55)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.)))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc new file mode 100644 index 0000000..b5bdeab --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -0,0 +1,339 @@ +/* + * 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_3d/scan_matching/fast_correlative_scan_matcher.h" + +#include +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" +#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +proto::FastCorrelativeScanMatcherOptions +CreateFastCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::FastCorrelativeScanMatcherOptions options; + options.set_branch_and_bound_depth( + parameter_dictionary->GetInt("branch_and_bound_depth")); + options.set_full_resolution_depth( + parameter_dictionary->GetInt("full_resolution_depth")); + options.set_rotational_histogram_size( + parameter_dictionary->GetInt("rotational_histogram_size")); + options.set_min_rotational_score( + parameter_dictionary->GetDouble("min_rotational_score")); + options.set_linear_xy_search_window( + parameter_dictionary->GetDouble("linear_xy_search_window")); + options.set_linear_z_search_window( + parameter_dictionary->GetDouble("linear_z_search_window")); + options.set_angular_search_window( + parameter_dictionary->GetDouble("angular_search_window")); + return options; +} + +class PrecomputationGridStack { + public: + PrecomputationGridStack( + const HybridGrid& hybrid_grid, + const proto::FastCorrelativeScanMatcherOptions& options, + const Eigen::Array3i& max_widths) { + CHECK_GE(options.branch_and_bound_depth(), 1); + CHECK_GE(options.full_resolution_depth(), 1); + precomputation_grids_.reserve(options.branch_and_bound_depth()); + precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid)); + Eigen::Array3i last_width = Eigen::Array3i::Ones(); + for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) { + const bool half_resolution = depth >= options.full_resolution_depth(); + const Eigen::Array3i next_width = + ((1 << depth) * Eigen::Array3i::Ones()).cwiseMin(max_widths); + const int full_voxels_per_high_resolution_voxel = + 1 << std::max(0, depth - options.full_resolution_depth()); + const Eigen::Array3i shift = + (next_width - last_width + + (full_voxels_per_high_resolution_voxel - 1)) / + full_voxels_per_high_resolution_voxel; + precomputation_grids_.push_back( + PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift)); + last_width = next_width; + } + } + + const PrecomputationGrid& Get(int depth) const { + return precomputation_grids_.at(depth); + } + + int max_depth() const { return precomputation_grids_.size() - 1; } + + private: + std::vector precomputation_grids_; +}; + +FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( + const HybridGrid& hybrid_grid, + const std::vector& nodes, + const proto::FastCorrelativeScanMatcherOptions& options) + : options_(options), + resolution_(hybrid_grid.resolution()), + linear_xy_window_size_( + common::RoundToInt(options_.linear_xy_search_window() / resolution_)), + linear_z_window_size_( + common::RoundToInt(options_.linear_z_search_window() / resolution_)), + precomputation_grid_stack_(common::make_unique( + hybrid_grid, options, Eigen::Array3i(1 + 2 * linear_xy_window_size_, + 1 + 2 * linear_xy_window_size_, + 1 + 2 * linear_z_window_size_))), + rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {} + +FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} + +bool FastCorrelativeScanMatcher::Match( + const transform::Rigid3d& initial_pose_estimate, + const sensor::PointCloud& coarse_point_cloud, + const sensor::PointCloud& fine_point_cloud, const float min_score, + float* score, transform::Rigid3d* pose_estimate) const { + CHECK_NOTNULL(score); + CHECK_NOTNULL(pose_estimate); + + const std::vector discrete_scans = + GenerateDiscreteScans(coarse_point_cloud, fine_point_cloud, + initial_pose_estimate.cast()); + + const std::vector lowest_resolution_candidates = + ComputeLowestResolutionCandidates(discrete_scans); + + const Candidate best_candidate = + BranchAndBound(discrete_scans, lowest_resolution_candidates, + precomputation_grid_stack_->max_depth(), min_score); + if (best_candidate.score > min_score) { + *score = best_candidate.score; + *pose_estimate = + (transform::Rigid3f( + resolution_ * best_candidate.offset.matrix().cast(), + Eigen::Quaternionf::Identity()) * + discrete_scans[best_candidate.scan_index].pose) + .cast(); + return true; + } + return false; +} + +DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( + const sensor::PointCloud& point_cloud, + const transform::Rigid3f& pose) const { + std::vector> cell_indices_per_depth; + const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0); + std::vector full_resolution_cell_indices; + for (const Eigen::Vector3f& point : + sensor::TransformPointCloud(point_cloud, pose)) { + full_resolution_cell_indices.push_back(original_grid.GetCellIndex(point)); + } + const int full_resolution_depth = std::min(options_.full_resolution_depth(), + options_.branch_and_bound_depth()); + CHECK_GE(full_resolution_depth, 1); + for (int i = 0; i != full_resolution_depth; ++i) { + cell_indices_per_depth.push_back(full_resolution_cell_indices); + } + const int low_resolution_depth = + options_.branch_and_bound_depth() - full_resolution_depth; + CHECK_GE(low_resolution_depth, 0); + const Eigen::Array3i search_window_start( + -linear_xy_window_size_, -linear_xy_window_size_, -linear_z_window_size_); + for (int i = 0; i != low_resolution_depth; ++i) { + const int reduction_exponent = i + 1; + const Eigen::Array3i low_resolution_search_window_start( + search_window_start[0] >> reduction_exponent, + search_window_start[1] >> reduction_exponent, + search_window_start[2] >> reduction_exponent); + cell_indices_per_depth.emplace_back(); + for (const Eigen::Array3i& cell_index : full_resolution_cell_indices) { + const Eigen::Array3i cell_at_start = cell_index + search_window_start; + const Eigen::Array3i low_resolution_cell_at_start( + cell_at_start[0] >> reduction_exponent, + cell_at_start[1] >> reduction_exponent, + cell_at_start[2] >> reduction_exponent); + cell_indices_per_depth.back().push_back( + low_resolution_cell_at_start - low_resolution_search_window_start); + } + } + return DiscreteScan{pose, cell_indices_per_depth}; +} + +std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( + const sensor::PointCloud& coarse_point_cloud, + const sensor::PointCloud& fine_point_cloud, + const transform::Rigid3f& initial_pose) const { + std::vector result; + // We set this value to something on the order of resolution to make sure that + // the std::acos() below is defined. + float max_scan_range = 3.f * resolution_; + for (const Eigen::Vector3f& point : coarse_point_cloud) { + const float range = point.norm(); + max_scan_range = std::max(range, max_scan_range); + } + const float kSafetyMargin = 1.f - 1e-2f; + const float angular_step_size = + kSafetyMargin * std::acos(1.f - + common::Pow2(resolution_) / + (2.f * common::Pow2(max_scan_range))); + const int angular_window_size = + common::RoundToInt(options_.angular_search_window() / angular_step_size); + // TODO(whess): Should there be a small search window for rotations around + // x and y? + std::vector angles; + for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) { + angles.push_back(rz * angular_step_size); + } + const std::vector scores = rotational_scan_matcher_.Match( + sensor::TransformPointCloud(fine_point_cloud, initial_pose), angles); + for (size_t i = 0; i != angles.size(); ++i) { + if (scores[i] < options_.min_rotational_score()) { + continue; + } + const Eigen::Vector3f angle_axis(0.f, 0.f, angles[i]); + // It's important to apply the 'angle_axis' rotation between the translation + // and rotation of the 'initial_pose', so that the rotation is around the + // origin of the laser scanner, and yaw is in map frame. + const transform::Rigid3f pose( + Eigen::Translation3f(initial_pose.translation()) * + transform::AngleAxisVectorToRotationQuaternion(angle_axis) * + Eigen::Quaternionf(initial_pose.rotation())); + result.push_back(DiscretizeScan(coarse_point_cloud, pose)); + } + return result; +} + +std::vector +FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( + const int num_discrete_scans) const { + const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); + const int num_lowest_resolution_linear_xy_candidates = + (2 * linear_xy_window_size_ + linear_step_size) / linear_step_size; + const int num_lowest_resolution_linear_z_candidates = + (2 * linear_z_window_size_ + linear_step_size) / linear_step_size; + const int num_candidates = + num_discrete_scans * + common::Power(num_lowest_resolution_linear_xy_candidates, 2) * + num_lowest_resolution_linear_z_candidates; + std::vector candidates; + candidates.reserve(num_candidates); + for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) { + for (int z = -linear_z_window_size_; z <= linear_z_window_size_; + z += linear_step_size) { + for (int y = -linear_xy_window_size_; y <= linear_xy_window_size_; + y += linear_step_size) { + for (int x = -linear_xy_window_size_; x <= linear_xy_window_size_; + x += linear_step_size) { + candidates.emplace_back(scan_index, Eigen::Array3i(x, y, z)); + } + } + } + } + CHECK_EQ(candidates.size(), num_candidates); + return candidates; +} + +void FastCorrelativeScanMatcher::ScoreCandidates( + const int depth, const std::vector& discrete_scans, + std::vector* const candidates) const { + const int reduction_exponent = + std::max(0, depth - options_.full_resolution_depth() + 1); + for (Candidate& candidate : *candidates) { + int sum = 0; + const DiscreteScan& discrete_scan = discrete_scans[candidate.scan_index]; + const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent, + candidate.offset[1] >> reduction_exponent, + candidate.offset[2] >> reduction_exponent); + CHECK_LT(depth, discrete_scan.cell_indices_per_depth.size()); + for (const Eigen::Array3i& cell_index : + discrete_scan.cell_indices_per_depth[depth]) { + const Eigen::Array3i proposed_cell_index = cell_index + offset; + sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index); + } + candidate.score = PrecomputationGrid::ToProbability( + sum / + static_cast(discrete_scan.cell_indices_per_depth[depth].size())); + } + std::sort(candidates->begin(), candidates->end(), std::greater()); +} + +std::vector +FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( + const std::vector& discrete_scans) const { + std::vector lowest_resolution_candidates = + GenerateLowestResolutionCandidates(discrete_scans.size()); + ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans, + &lowest_resolution_candidates); + return lowest_resolution_candidates; +} + +Candidate FastCorrelativeScanMatcher::BranchAndBound( + const std::vector& discrete_scans, + const std::vector& candidates, const int candidate_depth, + float min_score) const { + if (candidate_depth == 0) { + // Return the best candidate. + return *candidates.begin(); + } + + Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero()); + best_high_resolution_candidate.score = min_score; + for (const Candidate& candidate : candidates) { + if (candidate.score <= min_score) { + break; + } + std::vector higher_resolution_candidates; + const int half_width = 1 << (candidate_depth - 1); + for (int z : {0, half_width}) { + if (candidate.offset.z() + z > linear_z_window_size_) { + break; + } + for (int y : {0, half_width}) { + if (candidate.offset.y() + y > linear_xy_window_size_) { + break; + } + for (int x : {0, half_width}) { + if (candidate.offset.x() + x > linear_xy_window_size_) { + break; + } + higher_resolution_candidates.emplace_back( + candidate.scan_index, candidate.offset + Eigen::Array3i(x, y, z)); + } + } + } + ScoreCandidates(candidate_depth - 1, discrete_scans, + &higher_resolution_candidates); + best_high_resolution_candidate = + std::max(best_high_resolution_candidate, + BranchAndBound(discrete_scans, higher_resolution_candidates, + candidate_depth - 1, + best_high_resolution_candidate.score)); + } + return best_high_resolution_candidate; +} + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h b/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h new file mode 100644 index 0000000..fd14256 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h @@ -0,0 +1,122 @@ +/* + * 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. + */ + +// This is an implementation of a 3D branch-and-bound algorithm similar to +// mapping_2d::FastCorrelativeScanMatcher. + +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/port.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +proto::FastCorrelativeScanMatcherOptions +CreateFastCorrelativeScanMatcherOptions( + common::LuaParameterDictionary* parameter_dictionary); + +class PrecomputationGridStack; + +struct DiscreteScan { + transform::Rigid3f pose; + // Contains a vector of discretized scans for each 'depth'. + std::vector> cell_indices_per_depth; +}; + +struct Candidate { + Candidate(const int scan_index, const Eigen::Array3i& offset) + : scan_index(scan_index), offset(offset) {} + + // Index into the discrete scans vectors. + int scan_index; + + // Linear offset from the initial pose in cell indices. For lower resolution + // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth + // block of possibilities. + Eigen::Array3i offset; + + // Score, higher is better. + float score = 0.f; + + bool operator<(const Candidate& other) const { return score < other.score; } + bool operator>(const Candidate& other) const { return score > other.score; } +}; + +class FastCorrelativeScanMatcher { + public: + FastCorrelativeScanMatcher( + const HybridGrid& hybrid_grid, + const std::vector& nodes, + const proto::FastCorrelativeScanMatcherOptions& options); + ~FastCorrelativeScanMatcher(); + + FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete; + FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) = + delete; + + // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given an + // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) + // is possible, true is returned, and 'score' and 'pose_estimate' are updated + // with the result. 'fine_point_cloud' is used to compute the rotational scan + // matcher score. + bool Match(const transform::Rigid3d& initial_pose_estimate, + const sensor::PointCloud& coarse_point_cloud, + const sensor::PointCloud& fine_point_cloud, float min_score, + float* score, transform::Rigid3d* pose_estimate) const; + + private: + DiscreteScan DiscretizeScan(const sensor::PointCloud& point_cloud, + const transform::Rigid3f& pose) const; + std::vector GenerateDiscreteScans( + const sensor::PointCloud& coarse_point_cloud, + const sensor::PointCloud& fine_point_cloud, + const transform::Rigid3f& initial_pose) const; + std::vector GenerateLowestResolutionCandidates( + int num_discrete_scans) const; + void ScoreCandidates(int depth, + const std::vector& discrete_scans, + std::vector* const candidates) const; + std::vector ComputeLowestResolutionCandidates( + const std::vector& discrete_scans) const; + Candidate BranchAndBound(const std::vector& discrete_scans, + const std::vector& candidates, + int candidate_depth, float min_score) const; + + const proto::FastCorrelativeScanMatcherOptions options_; + const float resolution_; + const int linear_xy_window_size_; + const int linear_z_window_size_; + std::unique_ptr precomputation_grid_stack_; + RotationalScanMatcher rotational_scan_matcher_; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc new file mode 100644 index 0000000..e10f434 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -0,0 +1,117 @@ +/* + * 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_3d/scan_matching/fast_correlative_scan_matcher.h" + +#include +#include +#include +#include + +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping_3d/laser_fan_inserter.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { +namespace { + +proto::FastCorrelativeScanMatcherOptions +CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { + auto parameter_dictionary = common::MakeDictionary( + "return {" + "branch_and_bound_depth = " + + std::to_string(branch_and_bound_depth) + + ", " + "full_resolution_depth = " + + std::to_string(branch_and_bound_depth) + + ", " + "rotational_histogram_size = 30, " + "min_rotational_score = 0.1, " + "linear_xy_search_window = 0.8, " + "linear_z_search_window = 0.8, " + "angular_search_window = 0.3, " + "}"); + return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); +} + +mapping_3d::proto::LaserFanInserterOptions CreateLaserFanInserterTestOptions() { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "hit_probability = 0.7, " + "miss_probability = 0.4, " + "num_free_space_voxels = 5, " + "}"); + return CreateLaserFanInserterOptions(parameter_dictionary.get()); +} + +TEST(FastCorrelativeScanMatcherTest, CorrectPose) { + std::mt19937 prng(42); + std::uniform_real_distribution distribution(-1.f, 1.f); + LaserFanInserter laser_fan_inserter(CreateLaserFanInserterTestOptions()); + constexpr float kMinScore = 0.1f; + const auto options = CreateFastCorrelativeScanMatcherTestOptions(5); + + sensor::PointCloud point_cloud{ + Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f), + Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f), + Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f), + Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f), + Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f), + Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)}; + + for (int i = 0; i != 20; ++i) { + const float x = 0.7f * distribution(prng); + const float y = 0.7f * distribution(prng); + const float z = 0.7f * distribution(prng); + const float theta = 0.2f * distribution(prng); + const auto expected_pose = + transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) * + transform::Rigid3f::Rotation( + Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); + + HybridGrid hybrid_grid(0.05f /* resolution */, + Eigen::Vector3f(0.5f, 1.5f, 2.5f) /* origin */); + hybrid_grid.StartUpdate(); + laser_fan_inserter.Insert( + sensor::LaserFan3D{ + expected_pose.translation(), + sensor::TransformPointCloud(point_cloud, expected_pose), + {}}, + &hybrid_grid); + + FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {}, + options); + transform::Rigid3d pose_estimate; + float score; + EXPECT_TRUE(fast_correlative_scan_matcher.Match( + transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore, + &score, &pose_estimate)); + EXPECT_LT(kMinScore, score); + EXPECT_THAT(expected_pose, + transform::IsNearly(pose_estimate.cast(), 0.05f)) + << "Actual: " << transform::ToProto(pose_estimate).DebugString() + << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/interpolated_grid.h b/cartographer/cartographer/mapping_3d/scan_matching/interpolated_grid.h new file mode 100644 index 0000000..32b8b33 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/interpolated_grid.h @@ -0,0 +1,155 @@ +/* + * 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_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ + +#include + +#include "cartographer/mapping_3d/hybrid_grid.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +// Interpolates between HybridGrid probability voxels. We use the tricubic +// interpolation which interpolates the values and has vanishing derivative at +// these points. +// +// This class is templated to work with the autodiff that Ceres provides. +// For this reason, it is also important that the interpolation scheme be +// continuously differentiable. +class InterpolatedGrid { + public: + explicit InterpolatedGrid(const HybridGrid& hybrid_grid) + : hybrid_grid_(hybrid_grid) {} + + InterpolatedGrid(const InterpolatedGrid&) = delete; + InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; + + // Returns the interpolated probability at (x, y, z) of the HybridGrid + // used to perform the interpolation. + // + // This is a piecewise, continuously differentiable function. We use the + // scalar part of Jet parameters to select our interval below. It is the + // tensor product volume of piecewise cubic polynomials that interpolate + // the values, and have vanishing derivative at the interval boundaries. + template + T GetProbability(const T& x, const T& y, const T& z) const { + double x1, y1, z1, x2, y2, z2; + ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2); + + const Eigen::Array3i index1 = + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1)); + const double q111 = hybrid_grid_.GetProbability(index1); + const double q112 = + hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1)); + const double q121 = + hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0)); + const double q122 = + hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1)); + const double q211 = + hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0)); + const double q212 = + hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1)); + const double q221 = + hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0)); + const double q222 = + hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1)); + + const T normalized_x = (x - x1) / (x2 - x1); + const T normalized_y = (y - y1) / (y2 - y1); + const T normalized_z = (z - z1) / (z2 - z1); + + // Compute pow(..., 2) and pow(..., 3). Using pow() here is very expensive. + const T normalized_xx = normalized_x * normalized_x; + const T normalized_xxx = normalized_x * normalized_xx; + const T normalized_yy = normalized_y * normalized_y; + const T normalized_yyy = normalized_y * normalized_yy; + const T normalized_zz = normalized_z * normalized_z; + const T normalized_zzz = normalized_z * normalized_zz; + + // We first interpolate in z, then y, then x. All 7 times this uses the same + // scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2). + // The first polynomial is 1 at t=0, 0 at t=1, the second polynomial is 0 + // at t=0, 1 at t=1. Both polynomials have derivative zero at t=0 and t=1. + const T q11 = (q111 - q112) * normalized_zzz * 2. + + (q112 - q111) * normalized_zz * 3. + q111; + const T q12 = (q121 - q122) * normalized_zzz * 2. + + (q122 - q121) * normalized_zz * 3. + q121; + const T q21 = (q211 - q212) * normalized_zzz * 2. + + (q212 - q211) * normalized_zz * 3. + q211; + const T q22 = (q221 - q222) * normalized_zzz * 2. + + (q222 - q221) * normalized_zz * 3. + q221; + const T q1 = (q11 - q12) * normalized_yyy * 2. + + (q12 - q11) * normalized_yy * 3. + q11; + const T q2 = (q21 - q22) * normalized_yyy * 2. + + (q22 - q21) * normalized_yy * 3. + q21; + return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. + + q1; + } + + private: + template + void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z, + double* x1, double* y1, double* z1, + double* x2, double* y2, + double* z2) const { + const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z); + *x1 = lower.x(); + *y1 = lower.y(); + *z1 = lower.z(); + *x2 = lower.x() + hybrid_grid_.resolution(); + *y2 = lower.y() + hybrid_grid_.resolution(); + *z2 = lower.z() + hybrid_grid_.resolution(); + } + + // Center of the next lower voxel, i.e., not necessarily the voxel containing + // (x, y, z). For each dimension, the largest voxel index so that the + // corresponding center is at most the given coordinate. + Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y, + const double z) const { + // Center of the cell containing (x, y, z). + Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + // Move to the next lower voxel center. + if (center.x() > x) { + center.x() -= hybrid_grid_.resolution(); + } + if (center.y() > y) { + center.y() -= hybrid_grid_.resolution(); + } + if (center.z() > z) { + center.z() -= hybrid_grid_.resolution(); + } + return center; + } + + // Uses the scalar part of a Ceres Jet. + template + Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y, + const T& jet_z) const { + return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); + } + + const HybridGrid& hybrid_grid_; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc b/cartographer/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc new file mode 100644 index 0000000..e7c2478 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc @@ -0,0 +1,91 @@ +/* + * 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_3d/scan_matching/interpolated_grid.h" + +#include "Eigen/Core" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { +namespace { + +class InterpolatedGridTest : public ::testing::Test { + protected: + InterpolatedGridTest() + : hybrid_grid_(0.1f /* resolution */, + Eigen::Vector3f(1.5f, 2.5f, 3.5f) /* origin */), + interpolated_grid_(hybrid_grid_) { + for (const auto& point : + {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), + Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), + Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), + Eigen::Vector3f(-7.f, 3.f, 1.f)}) { + hybrid_grid_.SetProbability(hybrid_grid_.GetCellIndex(point), 1.); + } + } + + float GetHybridGridProbability(float x, float y, float z) const { + return hybrid_grid_.GetProbability( + hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + + HybridGrid hybrid_grid_; + InterpolatedGrid interpolated_grid_; +}; + +TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { + for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) { + for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { + for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { + EXPECT_NEAR(GetHybridGridProbability(x, y, z), + interpolated_grid_.GetProbability(x, y, z), 1e-6); + } + } + } +} + +TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { + const double kSampleStep = hybrid_grid_.resolution() / 10.; + for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) { + for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { + for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { + const float start_probability = GetHybridGridProbability(x, y, z); + const float next_probability = + GetHybridGridProbability(x + hybrid_grid_.resolution(), y, z); + const float grid_difference = next_probability - start_probability; + if (std::abs(grid_difference) < 1e-6f) { + continue; + } + for (double sample = kSampleStep; + sample < hybrid_grid_.resolution() - 2 * kSampleStep; + sample += kSampleStep) { + EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability( + x + sample + kSampleStep, y, z) - + interpolated_grid_.GetProbability( + x + sample, y, z))); + } + } + } + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h b/cartographer/cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h new file mode 100644 index 0000000..2f3435a --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ + +#include "Eigen/Core" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/scan_matching/interpolated_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +// Computes the cost of inserting occupied space described by the point cloud +// into the map. The cost increases with the amount of free space that would be +// replaced by occupied space. +class OccupiedSpaceCostFunctor { + public: + // Creates an OccupiedSpaceCostFunctor using the specified grid, 'rotation' to + // add to all poses, and point cloud. + OccupiedSpaceCostFunctor(const double scaling_factor, + const sensor::PointCloud& point_cloud, + const HybridGrid& hybrid_grid) + : scaling_factor_(scaling_factor), + point_cloud_(point_cloud), + interpolated_grid_(hybrid_grid) {} + + OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; + OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; + + template + bool operator()(const T* const translation, const T* const rotation, + T* const residual) const { + const transform::Rigid3 transform( + Eigen::Map>(translation), + Eigen::Quaternion(rotation[0], rotation[1], rotation[2], + rotation[3])); + return Evaluate(transform, residual); + } + + template + bool Evaluate(const transform::Rigid3& transform, + T* const residual) const { + for (size_t i = 0; i < point_cloud_.size(); ++i) { + const Eigen::Matrix world = + transform * point_cloud_[i].cast(); + const T probability = + interpolated_grid_.GetProbability(world[0], world[1], world[2]); + residual[i] = scaling_factor_ * (1. - probability); + } + return true; + } + + private: + const double scaling_factor_; + const sensor::PointCloud& point_cloud_; + const InterpolatedGrid interpolated_grid_; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid.cc b/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid.cc new file mode 100644 index 0000000..4f43677 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid.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_3d/scan_matching/precomputation_grid.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/math.h" +#include "cartographer/mapping/probability_values.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +namespace { + +// C++11 defines that integer division rounds towards zero. For index math, we +// actually need it to round towards negative infinity. Luckily bit shifts have +// that property. +inline int DivideByTwoRoundingTowardsNegativeInfinity(const int value) { + return value >> 1; +} + +// Computes the half resolution index corresponding to the full resolution +// 'cell_index'. +Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) { + return Eigen::Array3i( + DivideByTwoRoundingTowardsNegativeInfinity(cell_index[0]), + DivideByTwoRoundingTowardsNegativeInfinity(cell_index[1]), + DivideByTwoRoundingTowardsNegativeInfinity(cell_index[2])); +} + +} // namespace + +PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) { + PrecomputationGrid result(hybrid_grid.resolution(), hybrid_grid.origin()); + for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { + const int cell_value = common::RoundToInt( + (mapping::ValueToProbability(it.GetValue()) - + mapping::kMinProbability) * + (255.f / (mapping::kMaxProbability - mapping::kMinProbability))); + CHECK_GE(cell_value, 0); + CHECK_LE(cell_value, 255); + *result.mutable_value(it.GetCellIndex()) = cell_value; + } + return result; +} + +PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, + const bool half_resolution, + const Eigen::Array3i& shift) { + PrecomputationGrid result(grid.resolution(), grid.origin()); + for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) { + for (int i = 0; i != 8; ++i) { + // We use this value to update 8 values in the resulting grid, at + // position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}). + // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, + // this results in precomputation grids analogous to the 2D case. + const Eigen::Array3i cell_index = + it.GetCellIndex() - shift * PrecomputationGrid::GetOctant(i); + auto* const cell_value = result.mutable_value( + half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index); + *cell_value = std::max(it.GetValue(), *cell_value); + } + } + return result; +} + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid.h b/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid.h new file mode 100644 index 0000000..964f7bf --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid.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_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ + +#include "cartographer/mapping_3d/hybrid_grid.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +class PrecomputationGrid : public HybridGridBase { + public: + PrecomputationGrid(const float resolution, const Eigen::Vector3f& origin) + : HybridGridBase(resolution, origin) {} + + // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. + static float ToProbability(float value) { + return mapping::kMinProbability + + value * + ((mapping::kMaxProbability - mapping::kMinProbability) / 255.f); + } +}; + +// Converts a HybridGrid to a PrecomputationGrid representing the same data, +// but only using 8 bit instead of 2 x 16 bit. +PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid); + +// Returns a grid of the same resolution containing the maximum value of +// original voxels in 'grid'. This maximum is over the 8 voxels that have +// any combination of index components optionally increased by 'shift'. +// If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, and this +// is using the precomputed grid of one depth before, this results in +// precomputation grids analogous to the 2D case. +PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, + bool half_resolution, + const Eigen::Array3i& shift); + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc b/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc new file mode 100644 index 0000000..1ff78c7 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc @@ -0,0 +1,82 @@ +/* + * 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_3d/scan_matching/precomputation_grid.h" + +#include +#include +#include + +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { +namespace { + +TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { + HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f)); + + std::mt19937 rng(23847); + std::uniform_int_distribution coordinate_distribution(-50, 49); + std::uniform_real_distribution value_distribution( + mapping::kMinProbability, mapping::kMaxProbability); + for (int i = 0; i < 1000; ++i) { + const auto x = coordinate_distribution(rng); + const auto y = coordinate_distribution(rng); + const auto z = coordinate_distribution(rng); + const Eigen::Array3i cell_index(x, y, z); + hybrid_grid.SetProbability(cell_index, value_distribution(rng)); + } + + std::vector precomputed_grids; + for (int depth = 0; depth <= 3; ++depth) { + if (depth == 0) { + precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid)); + } else { + precomputed_grids.push_back( + PrecomputeGrid(precomputed_grids.back(), false, + (1 << (depth - 1)) * Eigen::Array3i::Ones())); + } + const int width = 1 << depth; + for (int i = 0; i < 100; ++i) { + const auto x = coordinate_distribution(rng); + const auto y = coordinate_distribution(rng); + const auto z = coordinate_distribution(rng); + float max_probability = 0.; + for (int dx = 0; dx < width; ++dx) { + for (int dy = 0; dy < width; ++dy) { + for (int dz = 0; dz < width; ++dz) { + max_probability = std::max( + max_probability, hybrid_grid.GetProbability( + Eigen::Array3i(x + dx, y + dy, z + dz))); + } + } + } + + EXPECT_NEAR(max_probability, + PrecomputationGrid::ToProbability( + precomputed_grids.back().value(Eigen::Array3i(x, y, z))), + 1e-2); + } + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/proto/CMakeLists.txt b/cartographer/cartographer/mapping_3d/scan_matching/proto/CMakeLists.txt new file mode 100644 index 0000000..bffac15 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/proto/CMakeLists.txt @@ -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. + +google_proto_library(mapping_3d_scan_matching_proto_ceres_scan_matcher_options + SRCS + ceres_scan_matcher_options.proto + DEPENDS + common_proto_ceres_solver_options +) + +google_proto_library(mapping_3d_scan_matching_proto_fast_correlative_scan_matcher_options + SRCS + fast_correlative_scan_matcher_options.proto +) diff --git a/cartographer/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto b/cartographer/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto new file mode 100644 index 0000000..3bbbd2a --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_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"; + +package cartographer.mapping_3d.scan_matching.proto; + +import "cartographer/common/proto/ceres_solver_options.proto"; + +// NEXT ID: 7 +message CeresScanMatcherOptions { + // Scaling parameters for each cost functor. + repeated double occupied_space_cost_functor_weight = 1; + optional double previous_pose_translation_delta_cost_functor_weight = 2; + optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; + + // Scale applied to the covariance estimate from Ceres. + optional double covariance_scale = 4; + + // Whether only to allow changes to yaw, keeping roll/pitch constant. + optional bool only_optimize_yaw = 5; + + // Configure the Ceres solver. See the Ceres documentation for more + // information: https://code.google.com/p/ceres-solver/ + optional common.proto.CeresSolverOptions ceres_solver_options = 6; +} diff --git a/cartographer/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto b/cartographer/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto new file mode 100644 index 0000000..db49cff --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_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_3d.scan_matching.proto; + +message FastCorrelativeScanMatcherOptions { + // Number of precomputed grids to use. + optional int32 branch_and_bound_depth = 2; + + // Number of full resolution grids to use, additional grids will reduce the + // resolution by half each. + optional int32 full_resolution_depth = 8; + + // Number of histogram buckets for the rotational scan matcher. + optional int32 rotational_histogram_size = 3; + + // Minimum score for the rotational scan matcher. + optional double min_rotational_score = 4; + + // Linear search window in the plane orthogonal to gravity in which the best + // possible scan alignment will be found. + optional double linear_xy_search_window = 5; + + // Linear search window in the gravity direction in which the best possible + // scan alignment will be found. + optional double linear_z_search_window = 6; + + // Minimum angular search window in which the best possible scan alignment + // will be found. + optional double angular_search_window = 7; +} diff --git a/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc b/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc new file mode 100644 index 0000000..9275aa7 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.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/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" + +#include + +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( + const mapping_2d::scan_matching::proto:: + RealTimeCorrelativeScanMatcherOptions& options) + : options_(options) {} + +float RealTimeCorrelativeScanMatcher::Match( + const transform::Rigid3d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid, + transform::Rigid3d* pose_estimate) const { + CHECK_NOTNULL(pose_estimate); + float best_score = -1.f; + for (const transform::Rigid3f& transform : GenerateExhaustiveSearchTransforms( + hybrid_grid.resolution(), point_cloud)) { + const transform::Rigid3f candidate = + initial_pose_estimate.cast() * transform; + const float score = ScoreCandidate( + hybrid_grid, sensor::TransformPointCloud(point_cloud, candidate), + transform); + if (score > best_score) { + best_score = score; + *pose_estimate = candidate.cast(); + } + } + return best_score; +} + +std::vector +RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms( + const float resolution, const sensor::PointCloud& point_cloud) const { + std::vector result; + const int linear_window_size = + common::RoundToInt(options_.linear_search_window() / resolution); + // We set this value to something on the order of resolution to make sure that + // the std::acos() below is defined. + float max_scan_range = 3.f * resolution; + for (const Eigen::Vector3f& point : point_cloud) { + const float range = point.norm(); + max_scan_range = std::max(range, max_scan_range); + } + const float kSafetyMargin = 1.f - 1e-3f; + const float angular_step_size = + kSafetyMargin * std::acos(1.f - + common::Pow2(resolution) / + (2.f * common::Pow2(max_scan_range))); + const int angular_window_size = + common::RoundToInt(options_.angular_search_window() / angular_step_size); + for (int z = -linear_window_size; z <= linear_window_size; ++z) { + for (int y = -linear_window_size; y <= linear_window_size; ++y) { + for (int x = -linear_window_size; x <= linear_window_size; ++x) { + for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) { + for (int ry = -angular_window_size; ry <= angular_window_size; ++ry) { + for (int rx = -angular_window_size; rx <= angular_window_size; + ++rx) { + const Eigen::Vector3f angle_axis(rx * angular_step_size, + ry * angular_step_size, + rz * angular_step_size); + result.emplace_back( + Eigen::Vector3f(x * resolution, y * resolution, + z * resolution), + transform::AngleAxisVectorToRotationQuaternion(angle_axis)); + } + } + } + } + } + } + return result; +} + +float RealTimeCorrelativeScanMatcher::ScoreCandidate( + const HybridGrid& hybrid_grid, + const sensor::PointCloud& transformed_point_cloud, + const transform::Rigid3f& transform) const { + float score = 0.f; + for (const Eigen::Vector3f& point : transformed_point_cloud) { + score += hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point)); + } + score /= static_cast(transformed_point_cloud.size()); + const float angle = transform::GetAngle(transform); + score *= + std::exp(-common::Pow2(transform.translation().norm() * + options_.translation_delta_cost_weight() + + angle * options_.rotation_delta_cost_weight())); + CHECK_GT(score, 0.f); + return score; +} + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h b/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h new file mode 100644 index 0000000..bdab139 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h @@ -0,0 +1,67 @@ +/* + * 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. + */ + +// A voxel accurate scan matcher, exhaustively evaluating the scan matching +// search space. +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +class RealTimeCorrelativeScanMatcher { + public: + explicit RealTimeCorrelativeScanMatcher( + const mapping_2d::scan_matching::proto:: + RealTimeCorrelativeScanMatcherOptions& options); + + RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) = + delete; + RealTimeCorrelativeScanMatcher& operator=( + const RealTimeCorrelativeScanMatcher&) = delete; + + // Aligns 'point_cloud' within the 'hybrid_grid' given an + // 'initial_pose_estimate' then updates 'pose_estimate' with the result and + // returns the score. + float Match(const transform::Rigid3d& initial_pose_estimate, + const sensor::PointCloud& point_cloud, + const HybridGrid& hybrid_grid, + transform::Rigid3d* pose_estimate) const; + + private: + std::vector GenerateExhaustiveSearchTransforms( + float resolution, const sensor::PointCloud& point_cloud) const; + float ScoreCandidate(const HybridGrid& hybrid_grid, + const sensor::PointCloud& transformed_point_cloud, + const transform::Rigid3f& transform) const; + + const mapping_2d::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions + options_; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc new file mode 100644 index 0000000..3c0aa22 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -0,0 +1,128 @@ +/* + * 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_3d/scan_matching/real_time_correlative_scan_matcher.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "cartographer/transform/transform.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { +namespace { + +class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { + protected: + RealTimeCorrelativeScanMatcherTest() + : hybrid_grid_(0.1f /* resolution */, + Eigen::Vector3f(0.5f, 0.5f, 0.5f) /* origin */), + expected_pose_(Eigen::Vector3d(-0.5, 0.5, 0.5), + Eigen::Quaterniond::Identity()) { + for (const auto& point : + {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), + Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), + Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), + Eigen::Vector3f(-7.f, 3.f, 1.f)}) { + point_cloud_.push_back(point); + hybrid_grid_.SetProbability( + hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); + } + + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + linear_search_window = 0.3, + angular_search_window = math.rad(1.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1., + covariance_scale = 1., + })text"); + real_time_correlative_scan_matcher_.reset( + new RealTimeCorrelativeScanMatcher( + mapping_2d::scan_matching:: + CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary.get()))); + } + + void TestFromInitialPose(const transform::Rigid3d& initial_pose) { + transform::Rigid3d pose; + + const float score = real_time_correlative_scan_matcher_->Match( + initial_pose, point_cloud_, hybrid_grid_, &pose); + LOG(INFO) << "Score: " << score; + EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3)); + } + + HybridGrid hybrid_grid_; + transform::Rigid3d expected_pose_; + sensor::PointCloud point_cloud_; + std::unique_ptr + real_time_correlative_scan_matcher_; +}; + +TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5))); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.3, 0.5, 0.5))); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.3))); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.4, 0.3, 0.7))); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.)))); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.)))); +} + +TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) { + TestFromInitialPose( + transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.)))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h b/cartographer/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h new file mode 100644 index 0000000..6f271cb --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.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_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/rotation.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +// Computes the cost of rotating the pose estimate. Cost increases with the +// solution's distance from the rotation estimate. +class RotationDeltaCostFunctor { + public: + // Constructs a new RotationDeltaCostFunctor from the given + // 'rotation_estimate'. + explicit RotationDeltaCostFunctor(const double scaling_factor, + const Eigen::Quaterniond& initial_rotation) + : scaling_factor_(scaling_factor) { + initial_rotation_inverse_[0] = initial_rotation.w(); + initial_rotation_inverse_[1] = -initial_rotation.x(); + initial_rotation_inverse_[2] = -initial_rotation.y(); + initial_rotation_inverse_[3] = -initial_rotation.z(); + } + + RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete; + RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete; + + template + bool operator()(const T* const rotation_quaternion, T* residual) const { + T delta[4]; + T initial_rotation_inverse[4] = { + T(initial_rotation_inverse_[0]), T(initial_rotation_inverse_[1]), + T(initial_rotation_inverse_[2]), T(initial_rotation_inverse_[3])}; + ceres::QuaternionProduct(initial_rotation_inverse, rotation_quaternion, + delta); + // Will compute the squared norm of the imaginary component of the delta + // quaternion which is sin(phi/2)^2. + residual[0] = scaling_factor_ * delta[1]; + residual[1] = scaling_factor_ * delta[2]; + residual[2] = scaling_factor_ * delta[3]; + return true; + } + + private: + const double scaling_factor_; + double initial_rotation_inverse_[4]; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc b/cartographer/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc new file mode 100644 index 0000000..5bbfd36 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.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_3d/scan_matching/rotational_scan_matcher.h" + +#include +#include + +#include "cartographer/common/math.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +namespace { + +constexpr float kMinDistance = 0.2f; +constexpr float kMaxDistance = 0.9f; +constexpr float kSliceHeight = 0.2f; + +void AddValueToHistogram(float angle, const float value, + Eigen::VectorXf* histogram) { + // Map the angle to [0, pi), i.e. a vector and its inverse are considered to + // represent the same angle. + while (angle > static_cast(M_PI)) { + angle -= static_cast(M_PI); + } + while (angle < 0.f) { + angle += static_cast(M_PI); + } + const float zero_to_one = angle / static_cast(M_PI); + const int bucket = common::Clamp( + common::RoundToInt(histogram->size() * zero_to_one - 0.5f), 0, + histogram->size() - 1); + (*histogram)(bucket) += value; +} + +Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) { + CHECK(!slice.empty()); + Eigen::Vector3f sum = Eigen::Vector3f::Zero(); + for (const Eigen::Vector3f& point : slice) { + sum += point; + } + return sum / static_cast(slice.size()); +} + +struct AngleValuePair { + float angle; + float value; +}; + +void AddPointCloudSliceToValueVector( + const sensor::PointCloud& slice, + std::vector* value_vector) { + if (slice.empty()) { + return; + } + // We compute the angle of the ray from the centroid to a laser return. + // If it is orthogonal to the angle we compute between laser returns, we will + // add the angle between laser returns to the histogram with the maximum + // weight. This is to reject, e.g., the angles observed on the ceiling and + // floor. + const Eigen::Vector3f centroid = ComputeCentroid(slice); + Eigen::Vector3f last_point = slice.front(); + for (const Eigen::Vector3f& point : slice) { + const Eigen::Vector2f delta = (point - last_point).head<2>(); + const Eigen::Vector2f direction = (point - centroid).head<2>(); + const float distance = delta.norm(); + if (distance < kMinDistance || direction.norm() < kMinDistance) { + continue; + } + if (distance > kMaxDistance) { + last_point = point; + continue; + } + const float angle = common::atan2(delta); + const float value = std::max( + 0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized()))); + value_vector->push_back(AngleValuePair{angle, value}); + } +} + +// A function to sort the points in each slice by angle around the centroid. +// This is because the returns from different lasers are interleaved in the +// data. +sensor::PointCloud SortSlice(const sensor::PointCloud& slice) { + struct SortableAnglePointPair { + bool operator<(const SortableAnglePointPair& rhs) const { + return angle < rhs.angle; + } + + float angle; + Eigen::Vector3f point; + }; + const Eigen::Vector3f centroid = ComputeCentroid(slice); + std::vector by_angle; + by_angle.reserve(slice.size()); + for (const Eigen::Vector3f& point : slice) { + const Eigen::Vector2f delta = (point - centroid).head<2>(); + if (delta.norm() < kMinDistance) { + continue; + } + by_angle.push_back(SortableAnglePointPair{common::atan2(delta), point}); + } + std::sort(by_angle.begin(), by_angle.end()); + sensor::PointCloud result; + for (const auto& pair : by_angle) { + result.push_back(pair.point); + } + return result; +} + +std::vector GetValuesForHistogram( + const sensor::PointCloud& point_cloud) { + std::map slices; + for (const Eigen::Vector3f& point : point_cloud) { + slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point); + } + std::vector result; + for (const auto& slice : slices) { + AddPointCloudSliceToValueVector(SortSlice(slice.second), &result); + } + return result; +} + +void AddValuesToHistogram(const std::vector& value_vector, + const float rotation, Eigen::VectorXf* histogram) { + for (const AngleValuePair& pair : value_vector) { + AddValueToHistogram(pair.angle + rotation, pair.value, histogram); + } +} + +} // namespace + +RotationalScanMatcher::RotationalScanMatcher( + const std::vector& nodes, const int histogram_size) + : histogram_(Eigen::VectorXf::Zero(histogram_size)) { + for (const mapping::TrajectoryNode& node : nodes) { + AddValuesToHistogram( + GetValuesForHistogram(sensor::TransformPointCloud( + node.constant_data->laser_fan_3d.returns.Decompress(), + node.pose.cast())), + 0.f, &histogram_); + } +} + +std::vector RotationalScanMatcher::Match( + const sensor::PointCloud& point_cloud, + const std::vector& angles) const { + std::vector result; + result.reserve(angles.size()); + const std::vector value_vector = + GetValuesForHistogram(point_cloud); + for (const float angle : angles) { + Eigen::VectorXf scan_histogram = Eigen::VectorXf::Zero(histogram_.size()); + AddValuesToHistogram(value_vector, angle, &scan_histogram); + result.push_back(MatchHistogram(scan_histogram)); + } + return result; +} + +float RotationalScanMatcher::MatchHistogram( + const Eigen::VectorXf& scan_histogram) const { + // We compute the dot product of normalized histograms as a measure of + // similarity. + const float scan_histogram_norm = scan_histogram.norm(); + const float histogram_norm = histogram_.norm(); + const float normalization = scan_histogram_norm * histogram_norm; + if (normalization < 1e-3f) { + return 1.f; + } + return histogram_.dot(scan_histogram) / normalization; +} + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h b/cartographer/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h new file mode 100644 index 0000000..26e8c3b --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h @@ -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. + */ + +#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ + +#include + +#include "Eigen/Geometry" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/sensor/point_cloud.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +class RotationalScanMatcher { + public: + explicit RotationalScanMatcher( + const std::vector& nodes, int histogram_size); + + RotationalScanMatcher(const RotationalScanMatcher&) = delete; + RotationalScanMatcher& operator=(const RotationalScanMatcher&) = delete; + + // Scores how well a 'point_cloud' can be understood as rotated by certain + // 'angles' relative to the 'nodes'. Each angle results in a score between + // 0 (worst) and 1 (best). + std::vector Match(const sensor::PointCloud& point_cloud, + const std::vector& angles) const; + + private: + float MatchHistogram(const Eigen::VectorXf& scan_histogram) const; + + Eigen::VectorXf histogram_; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ diff --git a/cartographer/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h b/cartographer/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h new file mode 100644 index 0000000..f7ac8b8 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h @@ -0,0 +1,64 @@ +/* + * 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_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ +#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ + +#include "Eigen/Core" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace mapping_3d { +namespace scan_matching { + +// Computes the cost of translating the initial pose estimate. Cost increases +// with the solution's distance from the initial estimate. +class TranslationDeltaCostFunctor { + public: + // Constructs a new TranslationDeltaCostFunctor from the given + // 'initial_pose_estimate'. + explicit TranslationDeltaCostFunctor( + const double scaling_factor, + const transform::Rigid3d& initial_pose_estimate) + : scaling_factor_(scaling_factor), + x_(initial_pose_estimate.translation().x()), + y_(initial_pose_estimate.translation().y()), + z_(initial_pose_estimate.translation().z()) {} + + TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; + TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = + delete; + + template + bool operator()(const T* const translation, T* residual) const { + residual[0] = scaling_factor_ * (translation[0] - x_); + residual[1] = scaling_factor_ * (translation[1] - y_); + residual[2] = scaling_factor_ * (translation[2] - z_); + return true; + } + + private: + const double scaling_factor_; + const double x_; + const double y_; + const double z_; +}; + +} // namespace scan_matching +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/cartographer/mapping_3d/sparse_pose_graph.cc new file mode 100644 index 0000000..854e46b --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph.cc @@ -0,0 +1,454 @@ +/* + * 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_3d/sparse_pose_graph.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/common/mutex.h" +#include "cartographer/mapping/proto/scan_matching_progress.pb.h" +#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" +#include "cartographer/sensor/voxel_filter.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +SparsePoseGraph::SparsePoseGraph( + const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool, + std::deque* constant_node_data) + : options_(options), + optimization_problem_(options_.optimization_problem_options()), + constraint_builder_(options_.constraint_builder_options(), thread_pool), + constant_node_data_(constant_node_data) {} + +SparsePoseGraph::~SparsePoseGraph() { + WaitForAllComputations(); + common::MutexLocker locker(&mutex_); + CHECK(scan_queue_ == nullptr); +} + +void SparsePoseGraph::GrowSubmapTransformsAsNeeded( + const std::vector& submaps) { + CHECK(!submaps.empty()); + CHECK_LT(submap_transforms_.size(), std::numeric_limits::max()); + const int next_transform_index = submap_transforms_.size(); + // Verify that we have an index for the first submap. + const int first_transform_index = GetSubmapIndex(submaps[0]); + if (submaps.size() == 1) { + // If we don't already have an entry for this submap, add one. + if (first_transform_index == next_transform_index) { + submap_transforms_.push_back(transform::Rigid3d::Identity()); + } + return; + } + CHECK_EQ(2, submaps.size()); + // CHECK that we have a index for the second submap. + const int second_transform_index = GetSubmapIndex(submaps[1]); + CHECK_LE(second_transform_index, next_transform_index); + // Extrapolate if necessary. + if (second_transform_index == next_transform_index) { + const auto& first_submap_transform = + submap_transforms_[first_transform_index]; + submap_transforms_.push_back(first_submap_transform * + submaps[0]->local_pose().inverse() * + submaps[1]->local_pose()); + } +} + +int SparsePoseGraph::AddScan( + common::Time time, const sensor::LaserFan3D& laser_fan_in_tracking, + const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance, const Submaps* submaps, + const Submap* const matching_submap, + const std::vector& insertion_submaps) { + const transform::Rigid3d optimized_pose(GetOdometryToMapTransform(*submaps) * + pose); + + common::MutexLocker locker(&mutex_); + const int j = trajectory_nodes_.size(); + CHECK_LT(j, std::numeric_limits::max()); + + constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{ + time, sensor::LaserFan{Eigen::Vector2f::Zero(), {}, {}}, + sensor::Compress(laser_fan_in_tracking), submaps, + transform::Rigid3d::Identity()}); + trajectory_nodes_.push_back( + mapping::TrajectoryNode{&constant_node_data_->back(), optimized_pose}); + trajectory_connectivity_.Add(submaps); + + if (submap_indices_.count(insertion_submaps.back()) == 0) { + submap_indices_.emplace(insertion_submaps.back(), + static_cast(submap_indices_.size())); + submap_states_.emplace_back(); + submap_states_.back().submap = insertion_submaps.back(); + submap_states_.back().trajectory = submaps; + CHECK_EQ(submap_states_.size(), submap_indices_.size()); + } + const Submap* const finished_submap = + insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr; + + // Make sure we have a sampler for this trajectory. + if (!global_localization_samplers_[submaps]) { + global_localization_samplers_[submaps] = + common::make_unique( + options_.global_sampling_ratio()); + } + + AddWorkItem([=]() REQUIRES(mutex_) { + ComputeConstraintsForScan(time, j, submaps, matching_submap, + insertion_submaps, finished_submap, pose, + covariance); + }); + return j; +} + +void SparsePoseGraph::AddWorkItem(std::function work_item) { + if (scan_queue_ == nullptr) { + work_item(); + } else { + scan_queue_->push_back(work_item); + } +} + +void SparsePoseGraph::AddImuData(common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) { + common::MutexLocker locker(&mutex_); + AddWorkItem([=]() REQUIRES(mutex_) { + optimization_problem_.AddImuData(time, linear_acceleration, + angular_velocity); + }); +} + +void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { + const int submap_index = GetSubmapIndex(submap); + const auto& node_data = optimization_problem_.node_data(); + CHECK_GT(node_data.size(), 0); + CHECK_LT(node_data.size(), std::numeric_limits::max()); + const int num_nodes = node_data.size(); + for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { + if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) { + const transform::Rigid3d relative_pose = + submap_transforms_[submap_index].inverse() * + node_data[scan_index].point_cloud_pose; + constraint_builder_.MaybeAddConstraint(submap_index, submap, scan_index, + trajectory_nodes_, relative_pose); + } + } +} + +void SparsePoseGraph::ComputeConstraintsForScan( + const common::Time time, const int scan_index, + const Submaps* scan_trajectory, const Submap* matching_submap, + std::vector insertion_submaps, const Submap* finished_submap, + const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) { + GrowSubmapTransformsAsNeeded(insertion_submaps); + const int matching_index = GetSubmapIndex(matching_submap); + const transform::Rigid3d optimized_pose = + submap_transforms_[matching_index] * + matching_submap->local_pose().inverse() * pose; + CHECK_EQ(scan_index, optimization_problem_.node_data().size()); + optimization_problem_.AddTrajectoryNode(time, pose, optimized_pose); + for (const Submap* submap : insertion_submaps) { + const int submap_index = GetSubmapIndex(submap); + CHECK(!submap_states_[submap_index].finished); + submap_states_[submap_index].scan_indices.emplace(scan_index); + // Unchanged covariance as (submap <- map) is a translation. + const transform::Rigid3d constraint_transform = + submap->local_pose().inverse() * pose; + constraints_.push_back(Constraint3D{ + submap_index, + scan_index, + {constraint_transform, + common::ComputeSpdMatrixSqrtInverse( + covariance, + options_.constraint_builder_options().max_covariance_trace(), + options_.constraint_builder_options() + .lower_covariance_eigenvalue_bound())}, + Constraint3D::INTRA_SUBMAP}); + } + + // Determine if this scan should be globally localized. + const bool run_global_localization = + global_localization_samplers_[scan_trajectory]->Pulse(); + + CHECK_LT(submap_states_.size(), std::numeric_limits::max()); + const int num_submaps = submap_states_.size(); + for (int submap_index = 0; submap_index != num_submaps; ++submap_index) { + if (submap_states_[submap_index].finished) { + CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0); + const transform::Rigid3d relative_pose = + submap_transforms_[submap_index].inverse() * optimized_pose; + + const auto* submap_trajectory = submap_states_[submap_index].trajectory; + + // Only globally match against submaps not in this trajectory. + if (run_global_localization && scan_trajectory != submap_trajectory) { + constraint_builder_.MaybeAddGlobalConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + scan_trajectory, submap_trajectory, &trajectory_connectivity_, + trajectory_nodes_); + } else { + const bool scan_and_submap_trajectories_connected = + reverse_connected_components_.count(scan_trajectory) > 0 && + reverse_connected_components_.count(submap_trajectory) > 0 && + reverse_connected_components_.at(scan_trajectory) == + reverse_connected_components_.at(submap_trajectory); + if (scan_trajectory == submap_trajectory || + scan_and_submap_trajectories_connected) { + constraint_builder_.MaybeAddConstraint( + submap_index, submap_states_[submap_index].submap, scan_index, + trajectory_nodes_, relative_pose); + } + } + } + } + + if (finished_submap != nullptr) { + const int finished_submap_index = GetSubmapIndex(finished_submap); + SubmapState& finished_submap_state = submap_states_[finished_submap_index]; + CHECK(!finished_submap_state.finished); + if (options_.also_match_to_new_submaps()) { + // We have a new completed submap, so we look into adding constraints for + // old scans. + ComputeConstraintsForOldScans(finished_submap); + } + finished_submap_state.finished = true; + } + constraint_builder_.NotifyEndOfScan(scan_index); + ++num_scans_since_last_loop_closure_; + if (options_.optimize_every_n_scans() > 0 && + num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { + CHECK(!run_loop_closure_); + run_loop_closure_ = true; + // If there is a 'scan_queue_' already, some other thread will take care. + if (scan_queue_ == nullptr) { + scan_queue_.reset(new std::deque>); + HandleScanQueue(); + } + } +} + +void SparsePoseGraph::HandleScanQueue() { + constraint_builder_.WhenDone( + [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + RunOptimization(); + + common::MutexLocker locker(&mutex_); + num_scans_since_last_loop_closure_ = 0; + run_loop_closure_ = false; + while (!run_loop_closure_) { + if (scan_queue_->empty()) { + LOG(INFO) << "We caught up. Hooray!"; + scan_queue_.reset(); + return; + } + scan_queue_->front()(); + scan_queue_->pop_front(); + } + // We have to optimize again. + HandleScanQueue(); + }); +} + +void SparsePoseGraph::WaitForAllComputations() { + bool notification = false; + common::MutexLocker locker(&mutex_); + const int num_finished_scans_at_start = + constraint_builder_.GetNumFinishedScans(); + while (!locker.AwaitWithTimeout( + [this]() REQUIRES(mutex_) { + return static_cast(constraint_builder_.GetNumFinishedScans()) == + trajectory_nodes_.size(); + }, + common::FromSeconds(1.))) { + std::ostringstream progress_info; + progress_info << "Optimizing: " << std::fixed << std::setprecision(1) + << 100. * (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / + (trajectory_nodes_.size() - + num_finished_scans_at_start) + << "%..."; + std::cout << "\r\x1b[K" << progress_info.str() << std::flush; + } + std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; + constraint_builder_.WhenDone([this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + common::MutexLocker locker(&mutex_); + notification = true; + }); + locker.Await([¬ification]() { return notification; }); +} + +void SparsePoseGraph::RunFinalOptimization() { + WaitForAllComputations(); + optimization_problem_.SetMaxNumIterations( + options_.max_num_final_iterations()); + RunOptimization(); +} + +void SparsePoseGraph::RunOptimization() { + if (!submap_transforms_.empty()) { + transform::Rigid3d submap_0_pose; + std::vector trajectories; + { + common::MutexLocker locker(&mutex_); + CHECK(!submap_states_.empty()); + submap_0_pose = submap_states_.front().submap->local_pose(); + for (const auto& trajectory_node : trajectory_nodes_) { + trajectories.push_back(trajectory_node.constant_data->trajectory); + } + } + + optimization_problem_.Solve(constraints_, submap_0_pose, trajectories, + &submap_transforms_); + common::MutexLocker locker(&mutex_); + has_new_optimized_poses_ = true; + + const auto& node_data = optimization_problem_.node_data(); + const size_t num_optimized_poses = node_data.size(); + for (size_t i = 0; i != num_optimized_poses; ++i) { + trajectory_nodes_[i].pose = + transform::Rigid3d(node_data[i].point_cloud_pose); + } + // Extrapolate all point cloud poses that were added later. + std::unordered_map + extrapolation_transforms; + for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { + const mapping::Submaps* trajectory = + trajectory_nodes_[i].constant_data->trajectory; + if (extrapolation_transforms.count(trajectory) == 0) { + extrapolation_transforms[trajectory] = transform::Rigid3d( + ExtrapolateSubmapTransforms(submap_transforms_, *trajectory) + .back() * + ExtrapolateSubmapTransforms(optimized_submap_transforms_, + *trajectory) + .back() + .inverse()); + } + trajectory_nodes_[i].pose = + extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; + } + optimized_submap_transforms_ = submap_transforms_; + connected_components_ = trajectory_connectivity_.ConnectedComponents(); + reverse_connected_components_.clear(); + for (size_t i = 0; i != connected_components_.size(); ++i) { + for (const auto* trajectory : connected_components_[i]) { + reverse_connected_components_.emplace(trajectory, i); + } + } + } +} + +bool SparsePoseGraph::HasNewOptimizedPoses() { + common::MutexLocker locker(&mutex_); + if (!has_new_optimized_poses_) { + return false; + } + has_new_optimized_poses_ = false; + return true; +} + +mapping::proto::ScanMatchingProgress +SparsePoseGraph::GetScanMatchingProgress() { + mapping::proto::ScanMatchingProgress progress; + common::MutexLocker locker(&mutex_); + progress.set_num_scans_total(trajectory_nodes_.size()); + progress.set_num_scans_finished(constraint_builder_.GetNumFinishedScans()); + return progress; +} + +std::vector SparsePoseGraph::GetTrajectoryNodes() { + common::MutexLocker locker(&mutex_); + return trajectory_nodes_; +} + +std::vector SparsePoseGraph::constraints_2d() { + return {}; +} + +std::vector SparsePoseGraph::constraints_3d() { + return constraints_; +} + +transform::Rigid3d SparsePoseGraph::GetOdometryToMapTransform( + const mapping::Submaps& submaps) { + return GetSubmapTransforms(submaps).back() * + submaps.Get(submaps.size() - 1)->local_pose().inverse(); +} + +std::vector> +SparsePoseGraph::GetConnectedTrajectories() { + common::MutexLocker locker(&mutex_); + return connected_components_; +} + +std::vector SparsePoseGraph::GetSubmapTransforms( + const mapping::Submaps& submaps) { + common::MutexLocker locker(&mutex_); + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps); +} + +std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( + const std::vector& submap_transforms, + const mapping::Submaps& submaps) const { + std::vector result; + int i = 0; + // Submaps for which we have optimized poses. + for (; i < submaps.size(); ++i) { + const mapping::Submap* submap = submaps.Get(i); + const auto submap_and_index = submap_indices_.find(submap); + // Determine if we have a loop-closed transform for this submap. + if (submap_and_index == submap_indices_.end() || + static_cast(submap_and_index->second) >= + submap_transforms.size()) { + break; + } + result.push_back(submap_transforms[submap_and_index->second]); + } + + // Extrapolate to the remaining submaps. + for (; i < submaps.size(); ++i) { + if (i == 0) { + result.push_back(transform::Rigid3d::Identity()); + continue; + } + result.push_back(result.back() * + submaps.Get(result.size() - 1)->local_pose().inverse() * + submaps.Get(result.size())->local_pose()); + } + return result; +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/cartographer/mapping_3d/sparse_pose_graph.h new file mode 100644 index 0000000..e4c8da6 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph.h @@ -0,0 +1,214 @@ +/* + * 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. + */ + +// Implements the loop closure method called Sparse Pose Adjustment (SPA) from +// 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. +// +// It is extended for submapping in 3D: +// Each scan has been matched against one or more submaps (adding a constraint +// for each match), both poses of scans and of submaps are to be optimized. +// All constraints are between a submap i and a scan j. + +#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ +#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ + +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/mutex.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/trajectory_connectivity.h" +#include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h" +#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping_3d { + +// Implements the SPA loop closure method. +class SparsePoseGraph : public mapping::SparsePoseGraph { + public: + SparsePoseGraph( + const mapping::proto::SparsePoseGraphOptions& options, + common::ThreadPool* thread_pool, + std::deque* constant_node_data); + ~SparsePoseGraph() override; + + SparsePoseGraph(const SparsePoseGraph&) = delete; + SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; + + // Adds a new 'laser_fan_in_tracking' observation at 'time', and a 'pose' + // that will later be optimized. The 'pose' was determined by scan matching + // against the 'matching_submap' and the scan was inserted into the + // 'insertion_submaps'. The index into the vector of trajectory nodes as + // used with GetTrajectoryNodes() is returned. + int AddScan(common::Time time, + const sensor::LaserFan3D& laser_fan_in_tracking, + const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& pose_covariance, + const Submaps* submaps, const Submap* matching_submap, + const std::vector& insertion_submaps) + EXCLUDES(mutex_); + + // Adds new IMU data to be used in the optimization. + void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity); + + void RunFinalOptimization() override; + bool HasNewOptimizedPoses() override; + mapping::proto::ScanMatchingProgress GetScanMatchingProgress() override; + std::vector> GetConnectedTrajectories() + override; + std::vector GetSubmapTransforms( + const mapping::Submaps& submaps) EXCLUDES(mutex_) override; + transform::Rigid3d GetOdometryToMapTransform(const mapping::Submaps& submaps) + EXCLUDES(mutex_) override; + std::vector GetTrajectoryNodes() override + EXCLUDES(mutex_); + std::vector constraints_2d() override; + std::vector constraints_3d() override; + + private: + struct SubmapState { + const Submap* submap = nullptr; + + // Indices of the scans that were inserted into this map together with + // constraints for them. They are not to be matched again when this submap + // becomes 'finished'. + std::set scan_indices; + + // Whether in the current state of the background thread this submap is + // finished. When this transitions to true, all scans are tried to match + // against this submap. Likewise, all new scans are matched against submaps + // which are finished. + bool finished = false; + + // The trajectory to which this SubmapState belongs. + const Submaps* trajectory = nullptr; + }; + + // Handles a new work item. + void AddWorkItem(std::function work_item) REQUIRES(mutex_); + + int GetSubmapIndex(const mapping::Submap* submap) const REQUIRES(mutex_) { + const auto iterator = submap_indices_.find(submap); + CHECK(iterator != submap_indices_.end()); + return iterator->second; + } + + // Grows 'submap_transforms_' to have an entry for every element of 'submaps'. + void GrowSubmapTransformsAsNeeded(const std::vector& submaps) + REQUIRES(mutex_); + + // Adds constraints for a scan, and starts scan matching in the background. + void ComputeConstraintsForScan( + common::Time time, int scan_index, const Submaps* scan_trajectory, + const Submap* matching_submap, + std::vector insertion_submaps, + const Submap* finished_submap, const transform::Rigid3d& pose, + const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_); + + // Adds constraints for older scans whenever a new submap is finished. + void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_); + + // Registers the callback to run the optimization once all constraints have + // been computed, that will also do all work that queue up in 'scan_queue_'. + void HandleScanQueue() REQUIRES(mutex_); + + // Waits until we caught up (i.e. nothing is waiting to be scheduled), and + // all computations have finished. + void WaitForAllComputations() EXCLUDES(mutex_); + + // Runs the optimization. Callers have to make sure, that there is only one + // optimization being run at a time. + void RunOptimization() EXCLUDES(mutex_); + + // Adds extrapolated transforms, so that there are transforms for all submaps. + std::vector ExtrapolateSubmapTransforms( + const std::vector& submap_transforms, + const mapping::Submaps& submaps) const REQUIRES(mutex_); + + const mapping::proto::SparsePoseGraphOptions options_; + common::Mutex mutex_; + + // If it exists, further scans must be added to this queue, and will be + // considered later. + std::unique_ptr>> scan_queue_ + GUARDED_BY(mutex_); + + // How our various trajectories are related. + mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); + + // We globally localize a fraction of the scans from each trajectory. + std::unordered_map> + global_localization_samplers_ GUARDED_BY(mutex_); + + // Number of scans added since last loop closure. + int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; + + // Whether the optimization has to be run before more data is added. + bool run_loop_closure_ GUARDED_BY(mutex_) = false; + + // Current optimization problem. + sparse_pose_graph::OptimizationProblem optimization_problem_; + sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); + std::vector constraints_; + std::vector submap_transforms_; // (map <- submap) + + // Submaps get assigned an index and state as soon as they are seen, even + // before they take part in the background computations. + std::map submap_indices_ GUARDED_BY(mutex_); + std::vector submap_states_ GUARDED_BY(mutex_); + + // Whether to return true on the next call to HasNewOptimizedPoses(). + bool has_new_optimized_poses_ GUARDED_BY(mutex_) = false; + + // Connectivity structure of our trajectories. + std::vector> connected_components_; + // Trajectory to connected component ID. + std::map reverse_connected_components_; + + // Data that are currently being shown. + // + // Deque to keep references valid for the background computation when adding + // new data. + std::deque* constant_node_data_; + std::vector trajectory_nodes_ GUARDED_BY(mutex_); + + // Current submap transforms used for displaying data. + std::vector optimized_submap_transforms_ + GUARDED_BY(mutex_); +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph/CMakeLists.txt b/cartographer/cartographer/mapping_3d/sparse_pose_graph/CMakeLists.txt new file mode 100644 index 0000000..88c847e --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph/CMakeLists.txt @@ -0,0 +1,80 @@ +# 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_library(mapping_3d_sparse_pose_graph_constraint_builder + USES_CERES + USES_EIGEN + SRCS + constraint_builder.cc + HDRS + constraint_builder.h + DEPENDS + common_fixed_ratio_sampler + common_histogram + common_lua_parameter_dictionary + common_make_unique + common_math + common_mutex + common_thread_pool + kalman_filter_pose_tracker + mapping_3d_scan_matching_ceres_scan_matcher + mapping_3d_scan_matching_fast_correlative_scan_matcher + mapping_3d_scan_matching_proto_ceres_scan_matcher_options + mapping_3d_scan_matching_proto_fast_correlative_scan_matcher_options + mapping_3d_sparse_pose_graph_optimization_problem + mapping_3d_submaps + mapping_submaps + mapping_trajectory_connectivity + mapping_trajectory_node + sensor_compressed_point_cloud + sensor_point_cloud + sensor_voxel_filter + transform_transform +) + +google_library(mapping_3d_sparse_pose_graph_optimization_problem + USES_CERES + USES_EIGEN + SRCS + optimization_problem.cc + HDRS + optimization_problem.h + DEPENDS + common_ceres_solver_options + common_lua_parameter_dictionary + common_make_unique + common_math + common_port + common_time + mapping_3d_acceleration_cost_function + mapping_3d_ceres_pose + mapping_3d_imu_integration + mapping_3d_rotation_cost_function + mapping_3d_submaps + mapping_sparse_pose_graph_proto_optimization_problem_options + transform_transform +) + +google_test(mapping_3d_sparse_pose_graph_optimization_problem_test + USES_CERES + USES_EIGEN + SRCS + optimization_problem_test.cc + DEPENDS + common_lua_parameter_dictionary_test_helpers + common_time + mapping_3d_sparse_pose_graph_optimization_problem + mapping_sparse_pose_graph_optimization_problem_options + transform_transform +) diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc new file mode 100644 index 0000000..013c01b --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -0,0 +1,330 @@ +/* + * 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_3d/sparse_pose_graph/constraint_builder.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Eigenvalues" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { +namespace sparse_pose_graph { + +namespace { + +std::vector ComputeSubmapNodes( + const std::vector& trajectory_nodes, + const Submap* const submap, int scan_index, + const transform::Rigid3d& initial_relative_pose) { + std::vector submap_nodes; + for (const int node_index : submap->trajectory_node_indices) { + submap_nodes.push_back(mapping::TrajectoryNode{ + trajectory_nodes[node_index].constant_data, + transform::Rigid3d(initial_relative_pose * + trajectory_nodes[scan_index].pose.inverse() * + trajectory_nodes[node_index].pose)}); + } + return submap_nodes; +} + +} // namespace + +ConstraintBuilder::ConstraintBuilder( + const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, + common::ThreadPool* const thread_pool) + : options_(options), + thread_pool_(thread_pool), + sampler_(options.sampling_ratio()), + adaptive_voxel_filter_(options.adaptive_voxel_filter_options()), + ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} + +ConstraintBuilder::~ConstraintBuilder() { + common::MutexLocker locker(&mutex_); + CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; + CHECK(pending_computations_.empty()); + CHECK_EQ(submap_queued_work_items_.size(), 0); + CHECK(when_done_ == nullptr); +} + +void ConstraintBuilder::MaybeAddConstraint( + const int submap_index, const Submap* const submap, const int scan_index, + const std::vector& trajectory_nodes, + const transform::Rigid3d& initial_relative_pose) { + if (initial_relative_pose.translation().norm() > + options_.max_constraint_distance()) { + return; + } + if (sampler_.Pulse()) { + const auto submap_nodes = ComputeSubmapNodes( + trajectory_nodes, submap, scan_index, initial_relative_pose); + common::MutexLocker locker(&mutex_); + CHECK_LE(scan_index, current_computation_); + constraints_.emplace_back(); + auto* const constraint = &constraints_.back(); + ++pending_computations_[current_computation_]; + const int current_computation = current_computation_; + const auto* const point_cloud = + &trajectory_nodes[scan_index].constant_data->laser_fan_3d.returns; + ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + submap_index, submap_nodes, &submap->high_resolution_hybrid_grid, + [=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_index, submap, scan_index, + nullptr, /* scan_trajectory */ + nullptr, /* submap_trajectory */ + false, /* match_full_submap */ + nullptr, /* trajectory_connectivity */ + point_cloud, initial_relative_pose, constraint); + FinishComputation(current_computation); + }); + } +} + +void ConstraintBuilder::MaybeAddGlobalConstraint( + const int submap_index, const Submap* const submap, const int scan_index, + const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const std::vector& trajectory_nodes) { + const auto submap_nodes = ComputeSubmapNodes( + trajectory_nodes, submap, scan_index, transform::Rigid3d::Identity()); + common::MutexLocker locker(&mutex_); + CHECK_LE(scan_index, current_computation_); + constraints_.emplace_back(); + auto* const constraint = &constraints_.back(); + ++pending_computations_[current_computation_]; + const int current_computation = current_computation_; + const auto* const point_cloud = + &trajectory_nodes[scan_index].constant_data->laser_fan_3d.returns; + ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + submap_index, submap_nodes, &submap->high_resolution_hybrid_grid, + [=]() EXCLUDES(mutex_) { + ComputeConstraint(submap_index, submap, scan_index, submap_trajectory, + scan_trajectory, true, /* match_full_submap */ + trajectory_connectivity, point_cloud, + transform::Rigid3d::Identity(), constraint); + FinishComputation(current_computation); + }); +} + +void ConstraintBuilder::NotifyEndOfScan(const int scan_index) { + common::MutexLocker locker(&mutex_); + CHECK_EQ(current_computation_, scan_index); + ++current_computation_; +} + +void ConstraintBuilder::WhenDone( + const std::function callback) { + common::MutexLocker locker(&mutex_); + CHECK(when_done_ == nullptr); + when_done_.reset(new std::function(callback)); + ++pending_computations_[current_computation_]; + const int current_computation = current_computation_; + thread_pool_->Schedule( + [this, current_computation] { FinishComputation(current_computation); }); +} + +Eigen::Matrix ConstraintBuilder::ComputeSqrtLambda( + const kalman_filter::PoseCovariance& covariance) const { + return common::ComputeSpdMatrixSqrtInverse( + covariance, options_.max_covariance_trace(), + options_.lower_covariance_eigenvalue_bound()); +} + +void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + const int submap_index, + const std::vector& submap_nodes, + const HybridGrid* const submap, const std::function work_item) { + if (submap_scan_matchers_[submap_index].fast_correlative_scan_matcher != + nullptr) { + thread_pool_->Schedule(work_item); + } else { + submap_queued_work_items_[submap_index].push_back(work_item); + if (submap_queued_work_items_[submap_index].size() == 1) { + thread_pool_->Schedule( + std::bind(std::mem_fn(&ConstraintBuilder::ConstructSubmapScanMatcher), + this, submap_index, submap_nodes, submap)); + } + } +} + +void ConstraintBuilder::ConstructSubmapScanMatcher( + const int submap_index, + const std::vector& submap_nodes, + const HybridGrid* const submap) { + auto submap_scan_matcher = + common::make_unique( + *submap, submap_nodes, + options_.fast_correlative_scan_matcher_options_3d()); + common::MutexLocker locker(&mutex_); + submap_scan_matchers_[submap_index] = {submap, + std::move(submap_scan_matcher)}; + for (const std::function& work_item : + submap_queued_work_items_[submap_index]) { + thread_pool_->Schedule(work_item); + } + submap_queued_work_items_.erase(submap_index); +} + +const ConstraintBuilder::SubmapScanMatcher* +ConstraintBuilder::GetSubmapScanMatcher(const int submap_index) { + common::MutexLocker locker(&mutex_); + const SubmapScanMatcher* submap_scan_matcher = + &submap_scan_matchers_[submap_index]; + CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr); + return submap_scan_matcher; +} + +void ConstraintBuilder::ComputeConstraint( + const int submap_index, const Submap* const submap, const int scan_index, + const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, bool match_full_submap, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const sensor::CompressedPointCloud* const compressed_point_cloud, + const transform::Rigid3d& initial_relative_pose, + std::unique_ptr* constraint) { + const transform::Rigid3d initial_pose = + submap->local_pose() * initial_relative_pose; + const SubmapScanMatcher* const submap_scan_matcher = + GetSubmapScanMatcher(submap_index); + const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress(); + const sensor::PointCloud filtered_point_cloud = + adaptive_voxel_filter_.Filter(point_cloud); + + // The 'constraint_transform' (i <- j) is computed from: + // - a 'filtered_point_cloud' in j, + // - the initial guess 'initial_pose' for (map <- j), + // - the result 'pose_estimate' of Match() (map <- j). + // - the submap->pose() (map <- i) + float score = 0.; + transform::Rigid3d pose_estimate = transform::Rigid3d::Identity(); + + CHECK(!match_full_submap) << "match_full_submap not supported for 3D."; + + if (!submap_scan_matcher->fast_correlative_scan_matcher->Match( + initial_pose, filtered_point_cloud, point_cloud, options_.min_score(), + &score, &pose_estimate)) { + return; + } + // We've reported a successful local match. + CHECK_GT(score, options_.min_score()); + { + common::MutexLocker locker(&mutex_); + score_histogram_.Add(score); + } + + // Use the CSM estimate as both the initial and previous pose. This has the + // effect that, in the absence of better information, we prefer the original + // CSM estimate. We also prefer to use the CSM covariance estimate for loop + // closure constraints since it is representative of a larger area (as opposed + // to the local Ceres estimate of covariance). + ceres::Solver::Summary unused_summary; + // TODO(hrapp): Compute covariance instead of relying on Ceres. + kalman_filter::PoseCovariance covariance; + ceres_scan_matcher_.Match( + pose_estimate, pose_estimate, + {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}}, + &pose_estimate, &covariance, &unused_summary); + + // 'covariance' is unchanged as (submap <- map) is a translation. + if (covariance.trace() > options_.max_covariance_trace()) { + return; + } + + const transform::Rigid3d constraint_transform = + submap->local_pose().inverse() * pose_estimate; + constraint->reset(new OptimizationProblem::Constraint{ + submap_index, + scan_index, + {constraint_transform, ComputeSqrtLambda(covariance)}, + OptimizationProblem::Constraint::INTER_SUBMAP}); + + if (options_.log_matches()) { + const transform::Rigid3d difference = + initial_pose.inverse() * pose_estimate; + std::ostringstream info; + info << "Scan index " << scan_index << " with " + << filtered_point_cloud.size() << " points on submap " << submap_index + << " differs by translation " << std::fixed << std::setprecision(2) + << difference.translation().norm() << " rotation " + << std::setprecision(3) << transform::GetAngle(difference) + << " with score " << std::setprecision(1) << 100. * score + << "% covariance trace " << std::scientific << std::setprecision(4) + << covariance.trace() << "."; + LOG(INFO) << info.str(); + } +} + +void ConstraintBuilder::FinishComputation(const int computation_index) { + Result result; + std::unique_ptr> callback; + { + common::MutexLocker locker(&mutex_); + if (--pending_computations_[computation_index] == 0) { + pending_computations_.erase(computation_index); + } + if (pending_computations_.empty()) { + CHECK_EQ(submap_queued_work_items_.size(), 0); + if (when_done_ != nullptr) { + for (const std::unique_ptr& + constraint : constraints_) { + if (constraint != nullptr) { + result.push_back(*constraint); + } + } + if (options_.log_matches()) { + LOG(INFO) << constraints_.size() << " computations resulted in " + << result.size() << " additional constraints."; + LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10); + } + constraints_.clear(); + callback = std::move(when_done_); + when_done_.reset(); + } + } + } + if (callback != nullptr) { + (*callback)(result); + } +} + +int ConstraintBuilder::GetNumFinishedScans() { + common::MutexLocker locker(&mutex_); + if (pending_computations_.empty()) { + return current_computation_; + } + return pending_computations_.begin()->first; +} + +} // namespace sparse_pose_graph +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h new file mode 100644 index 0000000..6547fbc --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.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_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ +#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ + +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/histogram.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/common/mutex.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/trajectory_connectivity.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/voxel_filter.h" + +namespace cartographer { +namespace mapping_3d { +namespace sparse_pose_graph { + +// Asynchronously computes constraints. +// +// Intermingle an arbitrary number of calls to MaybeAddConstraint() or +// MaybeAddGlobalConstraint, then call WhenDone(). After all computations are +// done the 'callback' will be called with the result and another +// MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. +// +// This class is thread-safe. +class ConstraintBuilder { + public: + using Constraint = mapping::SparsePoseGraph::Constraint3D; + using Result = std::vector; + + ConstraintBuilder( + const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& + options, + common::ThreadPool* thread_pool); + ~ConstraintBuilder(); + + ConstraintBuilder(const ConstraintBuilder&) = delete; + ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_index', and the 'laser_fan_3d.returns' in 'trajectory_nodes' for + // 'scan_index'. The 'initial_relative_pose' is relative to the 'submap'. + // + // The pointees of 'submap' and 'laser_fan_3d.returns' must stay valid until + // all computations are finished. + void MaybeAddConstraint( + int submap_index, const Submap* submap, int scan_index, + const std::vector& trajectory_nodes, + const transform::Rigid3d& initial_relative_pose); + + // Schedules exploring a new constraint between 'submap' identified by + // 'submap_index' and the 'laser_fan_3d.returns' in 'trajectory_nodes' for + // 'scan_index'. This performs full-submap matching. + // + // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and + // the 'submap' should be from 'submap_trajectory'. The + // 'trajectory_connectivity' is updated if the full-submap match succeeds. + // + // The pointees of 'submap' and 'point_cloud' must stay valid until all + // computations are finished. + void MaybeAddGlobalConstraint( + int submap_index, const Submap* submap, int scan_index, + const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const std::vector& trajectory_nodes); + + // Must be called after all computations related to 'scan_index' are added. + void NotifyEndOfScan(int scan_index); + + // Registers the 'callback' to be called with the results, after all + // computations triggered by MaybeAddConstraint() have finished. + void WhenDone(std::function callback); + + // Computes the inverse square root of 'covariance'. + Eigen::Matrix ComputeSqrtLambda( + const kalman_filter::PoseCovariance& covariance) const; + + // Returns the number of consecutive finished scans. + int GetNumFinishedScans(); + + private: + struct SubmapScanMatcher { + const HybridGrid* hybrid_grid; + std::unique_ptr + fast_correlative_scan_matcher; + }; + + // Either schedules the 'work_item', or if needed, schedules the scan matcher + // construction and queues the 'work_item'. + void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( + int submap_index, + const std::vector& submap_nodes, + const HybridGrid* submap, std::function work_item) + REQUIRES(mutex_); + + // Constructs the scan matcher for a 'submap', then schedules its work items. + void ConstructSubmapScanMatcher( + int submap_index, + const std::vector& submap_nodes, + const HybridGrid* submap) EXCLUDES(mutex_); + + // Returns the scan matcher for a submap, which has to exist. + const SubmapScanMatcher* GetSubmapScanMatcher(int submap_index) + EXCLUDES(mutex_); + + // Runs in a background thread and does computations for an additional + // constraint, assuming 'submap' and 'point_cloud' do not change anymore. + // If 'match_full_submap' is true, and global localization succeeds, will + // connect 'scan_trajectory' and 'submap_trajectory' in + // 'trajectory_connectivity'. + // As output, it may create a new Constraint in 'constraint'. + void ComputeConstraint( + int submap_index, const Submap* submap, int scan_index, + const mapping::Submaps* scan_trajectory, + const mapping::Submaps* submap_trajectory, bool match_full_submap, + mapping::TrajectoryConnectivity* trajectory_connectivity, + const sensor::CompressedPointCloud* const compressed_point_cloud, + const transform::Rigid3d& initial_relative_pose, + std::unique_ptr* constraint) EXCLUDES(mutex_); + + // Decrements the 'pending_computations_' count. If all computations are done, + // runs the 'when_done_' callback and resets the state. + void FinishComputation(int computation_index) EXCLUDES(mutex_); + + const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_; + common::ThreadPool* thread_pool_; + common::Mutex mutex_; + + // 'callback' set by WhenDone(). + std::unique_ptr> when_done_ + GUARDED_BY(mutex_); + + // Index of the scan in reaction to which computations are currently + // added. This is always the highest scan index seen so far, even when older + // scans are matched against a new submap. + int current_computation_ GUARDED_BY(mutex_) = 0; + + // For each added scan, maps to the number of pending computations that were + // added for it. + std::map pending_computations_ GUARDED_BY(mutex_); + + // Constraints currently being computed in the background. A deque is used to + // keep pointers valid when adding more entries. + std::deque> constraints_ GUARDED_BY(mutex_); + + // Map of already constructed scan matchers by 'submap_index'. + std::map submap_scan_matchers_ GUARDED_BY(mutex_); + + // Map by 'submap_index' of scan matchers under construction, and the work + // to do once construction is done. + std::map>> submap_queued_work_items_ + GUARDED_BY(mutex_); + + common::FixedRatioSampler sampler_; + const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_; + scan_matching::CeresScanMatcher ceres_scan_matcher_; + + // Histogram of scan matcher scores. + common::Histogram score_histogram_ GUARDED_BY(mutex_); +}; + +} // namespace sparse_pose_graph +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc new file mode 100644 index 0000000..860fa35 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -0,0 +1,286 @@ +/* + * 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_3d/sparse_pose_graph/optimization_problem.h" + +#include +#include +#include +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping_3d/acceleration_cost_function.h" +#include "cartographer/mapping_3d/ceres_pose.h" +#include "cartographer/mapping_3d/imu_integration.h" +#include "cartographer/mapping_3d/rotation_cost_function.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" +#include "ceres/rotation.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { +namespace sparse_pose_graph { + +namespace { + +struct ConstantYawQuaternionPlus { + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { + const T delta_norm = + ceres::sqrt(common::Pow2(delta[0]) + common::Pow2(delta[1])); + const T sin_delta_over_delta = + delta_norm < 1e-6 ? T(1.) : ceres::sin(delta_norm) / delta_norm; + T q_delta[4]; + q_delta[0] = delta_norm < 1e-6 ? T(1.) : ceres::cos(delta_norm); + q_delta[1] = sin_delta_over_delta * delta[0]; + q_delta[2] = sin_delta_over_delta * delta[1]; + q_delta[3] = T(0.); + // We apply the 'delta' which is interpreted as an angle-axis rotation + // vector in the xy-plane of the submap frame. This way we can align to + // gravity because rotations around the z-axis in the submap frame do not + // change gravity alignment, while disallowing random rotations of the map + // that have nothing to do with gravity alignment (i.e. we disallow steps + // just changing "yaw" of the complete map). + ceres::QuaternionProduct(x, q_delta, x_plus_delta); + return true; + } +}; + +} // namespace + +OptimizationProblem::OptimizationProblem( + const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& + options) + : options_(options) {} + +OptimizationProblem::~OptimizationProblem() {} + +template +std::array OptimizationProblem::SpaCostFunction::ComputeUnscaledError( + const transform::Rigid3d& zbar_ij, const T* const c_i_rotation, + const T* const c_i_translation, const T* const c_j_rotation, + const T* const c_j_translation) { + const Eigen::Quaternion R_i_inverse(c_i_rotation[0], -c_i_rotation[1], + -c_i_rotation[2], -c_i_rotation[3]); + + const Eigen::Matrix delta(c_j_translation[0] - c_i_translation[0], + c_j_translation[1] - c_i_translation[1], + c_j_translation[2] - c_i_translation[2]); + const Eigen::Matrix h_translation = R_i_inverse * delta; + + const Eigen::Quaternion h_rotation_inverse = + Eigen::Quaternion(c_j_rotation[0], -c_j_rotation[1], -c_j_rotation[2], + -c_j_rotation[3]) * + Eigen::Quaternion(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2], + c_i_rotation[3]); + + const Eigen::Matrix angle_axis_difference = + transform::RotationQuaternionToAngleAxisVector( + h_rotation_inverse * zbar_ij.rotation().cast()); + + return {{T(zbar_ij.translation().x()) - h_translation[0], + T(zbar_ij.translation().y()) - h_translation[1], + T(zbar_ij.translation().z()) - h_translation[2], + angle_axis_difference[0], angle_axis_difference[1], + angle_axis_difference[2]}}; +} + +template +void OptimizationProblem::SpaCostFunction::ComputeScaledError( + const Constraint::Pose& pose, const T* const c_i_rotation, + const T* const c_i_translation, const T* const c_j_rotation, + const T* const c_j_translation, T* const e) { + std::array e_ij = + ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation, + c_j_rotation, c_j_translation); + // Matrix-vector product of sqrt_Lambda_ij * e_ij + for (int s = 0; s != 6; ++s) { + e[s] = T(0.); + for (int t = 0; t != 6; ++t) { + e[s] += pose.sqrt_Lambda_ij(s, t) * e_ij[t]; + } + } +} + +template +bool OptimizationProblem::SpaCostFunction::operator()( + const T* const c_i_rotation, const T* const c_i_translation, + const T* const c_j_rotation, const T* const c_j_translation, T* e) const { + ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation, + c_j_translation, e); + return true; +} + +void OptimizationProblem::AddImuData(common::Time time, + const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity) { + imu_data_.push_back(ImuData{time, linear_acceleration, angular_velocity}); +} + +void OptimizationProblem::AddTrajectoryNode( + common::Time time, const transform::Rigid3d& initial_point_cloud_pose, + const transform::Rigid3d& point_cloud_pose) { + node_data_.push_back( + NodeData{time, initial_point_cloud_pose, point_cloud_pose}); +} + +void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { + options_.mutable_ceres_solver_options()->set_max_num_iterations( + max_num_iterations); +} + +void OptimizationProblem::Solve( + const std::vector& constraints, + const transform::Rigid3d& submap_0_transform, + const std::vector& trajectories, + std::vector* submap_transforms) { + if (node_data_.empty()) { + // Nothing to optimize. + return; + } + CHECK(!imu_data_.empty()); + + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + + // Set the starting point. + std::deque C_submaps; + std::deque C_point_clouds; + // Tie the first submap to the origin. + CHECK(!submap_transforms->empty()); + C_submaps.emplace_back( + transform::Rigid3d::Identity(), + common::make_unique>(), + &problem); + problem.SetParameterBlockConstant(C_submaps.back().translation()); + + for (size_t i = 1; i != submap_transforms->size(); ++i) { + C_submaps.emplace_back( + (*submap_transforms)[i], + common::make_unique(), &problem); + } + for (size_t j = 0; j != node_data_.size(); ++j) { + C_point_clouds.emplace_back( + node_data_[j].point_cloud_pose, + common::make_unique(), &problem); + } + + // Add cost functions for the loop closing constraints. + for (const Constraint& constraint : constraints) { + CHECK_GE(constraint.i, 0); + CHECK_LT(constraint.i, submap_transforms->size()); + CHECK_GE(constraint.j, 0); + CHECK_LT(constraint.j, node_data_.size()); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new SpaCostFunction(constraint.pose)), + new ceres::HuberLoss(options_.huber_scale()), + C_submaps[constraint.i].rotation(), + C_submaps[constraint.i].translation(), + C_point_clouds[constraint.j].rotation(), + C_point_clouds[constraint.j].translation()); + } + + CHECK(!node_data_.empty()); + CHECK_GE(trajectories.size(), node_data_.size()); + + // Add constraints for IMU observed data: angular velocities and + // accelerations. + auto it = imu_data_.cbegin(); + while ((it + 1) != imu_data_.cend() && (it + 1)->time <= node_data_[0].time) { + ++it; + } + + for (size_t j = 1; j < node_data_.size(); ++j) { + auto it2 = it; + const IntegrateImuResult result = IntegrateImu( + imu_data_, node_data_[j - 1].time, node_data_[j].time, &it); + if (j + 1 < node_data_.size()) { + const common::Duration first_delta_time = + node_data_[j].time - node_data_[j - 1].time; + const common::Duration second_delta_time = + node_data_[j + 1].time - node_data_[j].time; + const common::Time first_center = + node_data_[j - 1].time + first_delta_time / 2; + const common::Time second_center = + node_data_[j].time + second_delta_time / 2; + const IntegrateImuResult result_to_first_center = + IntegrateImu(imu_data_, node_data_[j - 1].time, first_center, &it2); + const IntegrateImuResult result_center_to_center = + IntegrateImu(imu_data_, first_center, second_center, &it2); + // 'delta_velocity' is the change in velocity from the point in time + // halfway between the first and second poses to halfway between second + // and third pose. It is computed from IMU data and still contains a + // delta due to gravity. The orientation of this vector is in the IMU + // frame at the second pose. + const Eigen::Vector3d delta_velocity = + (result.delta_rotation.inverse() * + result_to_first_center.delta_rotation) * + result_center_to_center.delta_velocity; + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction(new AccelerationCostFunction( + options_.acceleration_scale(), delta_velocity, + common::ToSeconds(first_delta_time), + common::ToSeconds(second_delta_time))), + nullptr, C_point_clouds[j].rotation(), + C_point_clouds[j - 1].translation(), C_point_clouds[j].translation(), + C_point_clouds[j + 1].translation(), &gravity_constant_); + } + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction( + new RotationCostFunction(options_.rotation_scale(), + result.delta_rotation)), + nullptr, C_point_clouds[j - 1].rotation(), + C_point_clouds[j].rotation()); + } + + // Solve. + ceres::Solver::Summary summary; + ceres::Solver::Options ceres_solver_options = + common::CreateCeresSolverOptions(options_.ceres_solver_options()); + ceres::Solve(ceres_solver_options, &problem, &summary); + + if (options_.log_solver_summary()) { + LOG(INFO) << summary.FullReport(); + LOG(INFO) << "Gravity was: " << gravity_constant_; + } + + // Store the result. + for (size_t i = 0; i != submap_transforms->size(); ++i) { + (*submap_transforms)[i] = C_submaps[i].ToRigid(); + } + for (size_t j = 0; j != node_data_.size(); ++j) { + node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid(); + } +} + +const std::vector& OptimizationProblem::node_data() const { + return node_data_; +} + +} // namespace sparse_pose_graph +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h new file mode 100644 index 0000000..a97edb5 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -0,0 +1,114 @@ +/* + * 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_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ +#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ + +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" +#include "cartographer/mapping_3d/imu_integration.h" +#include "cartographer/mapping_3d/submaps.h" + +namespace cartographer { +namespace mapping_3d { +namespace sparse_pose_graph { + +struct NodeData { + common::Time time; + transform::Rigid3d initial_point_cloud_pose; + transform::Rigid3d point_cloud_pose; +}; + +// Implements the SPA loop closure method. +class OptimizationProblem { + public: + using Constraint = mapping::SparsePoseGraph::Constraint3D; + + explicit OptimizationProblem( + const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& + options); + ~OptimizationProblem(); + + OptimizationProblem(const OptimizationProblem&) = delete; + OptimizationProblem& operator=(const OptimizationProblem&) = delete; + + void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity); + void AddTrajectoryNode(common::Time time, + const transform::Rigid3d& initial_point_cloud_pose, + const transform::Rigid3d& point_cloud_pose); + + void SetMaxNumIterations(int32 max_num_iterations); + + // Computes the optimized poses. The point cloud at 'point_cloud_poses[i]' + // belongs to 'trajectories[i]'. Within a given trajectory, scans are expected + // to be contiguous. + void Solve(const std::vector& constraints, + const transform::Rigid3d& submap_0_transform, + const std::vector& trajectories, + std::vector* submap_transforms); + + const std::vector& node_data() const; + + private: + class SpaCostFunction { + public: + explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} + + // Compute the error (linear offset and rotational error) without scaling + // it by the covariance. + template + static std::array ComputeUnscaledError( + const transform::Rigid3d& zbar_ij, const T* const c_i_rotation, + const T* const c_i_translation, const T* const c_j_rotation, + const T* const c_j_translation); + + // Computes the error scaled by 'sqrt_Lambda_ij', storing it in 'e'. + template + static void ComputeScaledError(const Constraint::Pose& pose, + const T* const c_i_rotation, + const T* const c_i_translation, + const T* const c_j_rotation, + const T* const c_j_translation, T* const e); + template + bool operator()(const T* const c_i_rotation, const T* const c_i_translation, + const T* const c_j_rotation, const T* const c_j_translation, + T* const e) const; + + private: + const Constraint::Pose pose_; + }; + + mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; + std::deque imu_data_; + std::vector node_data_; + double gravity_constant_ = 9.8; +}; + +} // namespace sparse_pose_graph +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ diff --git a/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc new file mode 100644 index 0000000..022e416 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -0,0 +1,186 @@ +/* + * 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_3d/sparse_pose_graph/optimization_problem.h" + +#include + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping_3d { +namespace sparse_pose_graph { +namespace { + +class OptimizationProblemTest : public ::testing::Test { + protected: + OptimizationProblemTest() + : optimization_problem_(CreateOptions()), rng_(45387) {} + + mapping::sparse_pose_graph::proto::OptimizationProblemOptions + CreateOptions() { + auto parameter_dictionary = common::MakeDictionary(R"text( + return { + acceleration_scale = 1e-4, + rotation_scale = 1e-2, + huber_scale = 1., + consecutive_scan_translation_penalty_factor = 1e-2, + consecutive_scan_rotation_penalty_factor = 1e-2, + log_solver_summary = true, + log_residual_histograms = true, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 200, + num_threads = 4, + }, + })text"); + return mapping::sparse_pose_graph::CreateOptimizationProblemOptions( + parameter_dictionary.get()); + } + + transform::Rigid3d RandomTransform(double translation_size, + double rotation_size) { + std::uniform_real_distribution translation_distribution( + -translation_size, translation_size); + const double x = translation_distribution(rng_); + const double y = translation_distribution(rng_); + const double z = translation_distribution(rng_); + std::uniform_real_distribution rotation_distribution(-rotation_size, + rotation_size); + const double rx = rotation_distribution(rng_); + const double ry = rotation_distribution(rng_); + const double rz = rotation_distribution(rng_); + return transform::Rigid3d(Eigen::Vector3d(x, y, z), + transform::AngleAxisVectorToRotationQuaternion( + Eigen::Vector3d(rx, ry, rz))); + } + + transform::Rigid3d RandomYawOnlyTransform(double translation_size, + double rotation_size) { + std::uniform_real_distribution translation_distribution( + -translation_size, translation_size); + const double x = translation_distribution(rng_); + const double y = translation_distribution(rng_); + const double z = translation_distribution(rng_); + std::uniform_real_distribution rotation_distribution(-rotation_size, + rotation_size); + const double rz = rotation_distribution(rng_); + return transform::Rigid3d(Eigen::Vector3d(x, y, z), + transform::AngleAxisVectorToRotationQuaternion( + Eigen::Vector3d(0., 0., rz))); + } + + OptimizationProblem optimization_problem_; + std::mt19937 rng_; +}; + +transform::Rigid3d AddNoise(const transform::Rigid3d& transform, + const transform::Rigid3d& noise) { + const Eigen::Quaterniond noisy_rotation(noise.rotation() * + transform.rotation()); + return transform::Rigid3d(transform.translation() + noise.translation(), + noisy_rotation); +} + +TEST_F(OptimizationProblemTest, ReducesNoise) { + constexpr int kNumNodes = 100; + const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity(); + const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation( + Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); + const mapping::Submaps* const kTrajectory = nullptr; + + struct NoisyNode { + transform::Rigid3d ground_truth_pose; + transform::Rigid3d noise; + }; + std::vector test_data; + for (int j = 0; j != kNumNodes; ++j) { + test_data.push_back( + NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)}); + } + + std::vector trajectories; + common::Time now = common::FromUniversal(0); + for (const NoisyNode& node : test_data) { + trajectories.push_back(kTrajectory); + const transform::Rigid3d pose = + AddNoise(node.ground_truth_pose, node.noise); + optimization_problem_.AddImuData(now, Eigen::Vector3d::UnitZ() * 9.81, + Eigen::Vector3d::Zero()); + optimization_problem_.AddTrajectoryNode(now, pose, pose); + now += common::FromSeconds(0.01); + } + + std::vector constraints; + for (int j = 0; j != kNumNodes; ++j) { + constraints.push_back(OptimizationProblem::Constraint{ + 0, j, OptimizationProblem::Constraint::Pose{ + AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), + Eigen::Matrix::Identity()}}); + // We add an additional independent, but equally noisy observation. + constraints.push_back(OptimizationProblem::Constraint{ + 1, j, OptimizationProblem::Constraint::Pose{ + AddNoise(test_data[j].ground_truth_pose, + RandomYawOnlyTransform(0.2, 0.3)), + Eigen::Matrix::Identity()}}); + // We add very noisy data with high covariance (i.e. small Lambda) to verify + // it is mostly ignored. + constraints.push_back(OptimizationProblem::Constraint{ + 2, j, OptimizationProblem::Constraint::Pose{ + kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * + RandomTransform(1e3, 3.), + 1e-9 * Eigen::Matrix::Identity()}}); + } + + std::vector submap_transforms = { + kSubmap0Transform, kSubmap0Transform, kSubmap2Transform}; + optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories, + &submap_transforms); + + double translation_error_before = 0.; + double translation_error_after = 0.; + double rotation_error_before = 0.; + double rotation_error_after = 0.; + const auto& node_data = optimization_problem_.node_data(); + for (int j = 0; j != kNumNodes; ++j) { + translation_error_before += + (test_data[j].ground_truth_pose.translation() - + node_data[j].initial_point_cloud_pose.translation()) + .norm(); + translation_error_after += (test_data[j].ground_truth_pose.translation() - + node_data[j].point_cloud_pose.translation()) + .norm(); + rotation_error_before += + transform::GetAngle(test_data[j].ground_truth_pose.inverse() * + node_data[j].initial_point_cloud_pose); + rotation_error_after += + transform::GetAngle(test_data[j].ground_truth_pose.inverse() * + node_data[j].point_cloud_pose); + } + EXPECT_GT(0.8 * translation_error_before, translation_error_after); + EXPECT_GT(0.8 * rotation_error_before, rotation_error_after); +} + +} // namespace +} // namespace sparse_pose_graph +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/submaps.cc b/cartographer/cartographer/mapping_3d/submaps.cc new file mode 100644 index 0000000..d41d3de --- /dev/null +++ b/cartographer/cartographer/mapping_3d/submaps.cc @@ -0,0 +1,411 @@ +/* + * 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_3d/submaps.h" + +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" +#include "cartographer/sensor/laser.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping_3d { + +namespace { + +constexpr float kSliceHalfHeight = 0.1f; + +struct LaserSegment { + Eigen::Vector2f from; + Eigen::Vector2f to; + bool hit; // Whether there is a laser return at 'to'. +}; + +// We compute a slice around the xy-plane. 'transform' is applied to the laser +// rays in global map frame to allow choosing an arbitrary slice. +void GenerateSegmentForSlice(const sensor::LaserFan3D& laser_fan_3d, + const transform::Rigid3f& pose, + const transform::Rigid3f& transform, + std::vector* segments) { + const sensor::LaserFan3D laser_fan = + sensor::TransformLaserFan3D(laser_fan_3d, transform * pose); + segments->reserve(laser_fan.returns.size()); + for (const Eigen::Vector3f& hit : laser_fan.returns) { + const Eigen::Vector2f laser_origin_xy = laser_fan.origin.head<2>(); + const float laser_origin_z = laser_fan.origin.z(); + const float delta_z = hit.z() - laser_origin_z; + const Eigen::Vector2f delta_xy = hit.head<2>() - laser_origin_xy; + if (laser_origin_z < -kSliceHalfHeight) { + // Laser ray originates below the slice. + if (hit.z() > kSliceHalfHeight) { + // Laser ray is cutting through the slice. + segments->push_back(LaserSegment{ + laser_origin_xy + + (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + laser_origin_xy + + (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + false}); + } else if (hit.z() > -kSliceHalfHeight) { + // Laser return is inside the slice. + segments->push_back(LaserSegment{ + laser_origin_xy + + (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + hit.head<2>(), true}); + } + } else if (laser_origin_z < kSliceHalfHeight) { + // Laser ray originates inside the slice. + if (hit.z() < -kSliceHalfHeight) { + // Laser hit is below. + segments->push_back(LaserSegment{ + laser_origin_xy, + laser_origin_xy + + (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + false}); + } else if (hit.z() < kSliceHalfHeight) { + // Full ray is inside the slice. + segments->push_back(LaserSegment{laser_origin_xy, hit.head<2>(), true}); + } else { + // Laser hit is above. + segments->push_back(LaserSegment{ + laser_origin_xy, + laser_origin_xy + + (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + false}); + } + } else { + // Laser ray originates above the slice. + if (hit.z() < -kSliceHalfHeight) { + // Laser ray is cutting through the slice. + segments->push_back(LaserSegment{ + laser_origin_xy + + (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + laser_origin_xy + + (-kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + false}); + } else if (hit.z() < kSliceHalfHeight) { + // Laser return is inside the slice. + segments->push_back(LaserSegment{ + laser_origin_xy + + (kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy, + hit.head<2>(), true}); + } + } + } +} + +void UpdateFreeSpaceFromSegment(const LaserSegment& segment, + const std::vector& miss_table, + mapping_2d::ProbabilityGrid* result) { + Eigen::Array2i from = result->limits().GetXYIndexOfCellContainingPoint( + segment.from.x(), segment.from.y()); + Eigen::Array2i to = result->limits().GetXYIndexOfCellContainingPoint( + segment.to.x(), segment.to.y()); + bool large_delta_y = + std::abs(to.y() - from.y()) > std::abs(to.x() - from.x()); + if (large_delta_y) { + std::swap(from.x(), from.y()); + std::swap(to.x(), to.y()); + } + if (from.x() > to.x()) { + std::swap(from, to); + } + const int dx = to.x() - from.x(); + const int dy = std::abs(to.y() - from.y()); + int error = dx / 2; + const int direction = (from.y() < to.y()) ? 1 : -1; + + for (; from.x() < to.x(); ++from.x()) { + if (large_delta_y) { + result->ApplyLookupTable(Eigen::Array2i(from.y(), from.x()), miss_table); + } else { + result->ApplyLookupTable(from, miss_table); + } + error -= dy; + if (error < 0) { + from.y() += direction; + error += dx; + } + } +} + +void InsertSegmentsIntoProbabilityGrid( + const std::vector& segments, + const std::vector& hit_table, const std::vector& miss_table, + mapping_2d::ProbabilityGrid* result) { + result->StartUpdate(); + if (segments.empty()) { + return; + } + Eigen::Vector2f min = segments.front().from; + Eigen::Vector2f max = min; + for (const LaserSegment& segment : segments) { + min = min.cwiseMin(segment.from); + min = min.cwiseMin(segment.to); + max = max.cwiseMax(segment.from); + max = max.cwiseMax(segment.to); + } + const float padding = 10. * result->limits().resolution(); + max += Eigen::Vector2f(padding, padding); + min -= Eigen::Vector2f(padding, padding); + result->GrowLimits(min.x(), min.y()); + result->GrowLimits(max.x(), max.y()); + + for (const LaserSegment& segment : segments) { + if (segment.hit) { + result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint( + segment.to.x(), segment.to.y()), + hit_table); + } + } + for (const LaserSegment& segment : segments) { + UpdateFreeSpaceFromSegment(segment, miss_table, result); + } +} + +} // namespace + +void InsertIntoProbabilityGrid( + const sensor::LaserFan3D& laser_fan_3d, const transform::Rigid3f& pose, + const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, + mapping_2d::ProbabilityGrid* result) { + std::vector segments; + GenerateSegmentForSlice( + laser_fan_3d, pose, + transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()), + &segments); + InsertSegmentsIntoProbabilityGrid(segments, laser_fan_inserter.hit_table(), + laser_fan_inserter.miss_table(), result); +} + +proto::SubmapsOptions CreateSubmapsOptions( + common::LuaParameterDictionary* parameter_dictionary) { + proto::SubmapsOptions options; + options.set_high_resolution( + parameter_dictionary->GetDouble("high_resolution")); + options.set_high_resolution_max_range( + parameter_dictionary->GetDouble("high_resolution_max_range")); + options.set_low_resolution(parameter_dictionary->GetDouble("low_resolution")); + options.set_num_laser_fans( + parameter_dictionary->GetNonNegativeInt("num_laser_fans")); + *options.mutable_laser_fan_inserter_options() = CreateLaserFanInserterOptions( + parameter_dictionary->GetDictionary("laser_fan_inserter").get()); + CHECK_GT(options.num_laser_fans(), 0); + return options; +} + +Submap::Submap(const float high_resolution, const float low_resolution, + const Eigen::Vector3f& origin, const int begin_laser_fan_index) + : mapping::Submap(origin, begin_laser_fan_index), + high_resolution_hybrid_grid(high_resolution, origin), + low_resolution_hybrid_grid(low_resolution, origin) {} + +Submaps::Submaps(const proto::SubmapsOptions& options) + : options_(options), + laser_fan_inserter_(options.laser_fan_inserter_options()) { + // We always want to have at least one likelihood field which we can return, + // and will create it at the origin in absence of a better choice. + AddSubmap(Eigen::Vector3f::Zero()); +} + +const Submap* Submaps::Get(int index) const { + CHECK_GE(index, 0); + CHECK_LT(index, size()); + return submaps_[index].get(); +} + +int Submaps::size() const { return submaps_.size(); } + +void Submaps::SubmapToProto( + int index, const std::vector& trajectory_nodes, + const transform::Rigid3d& global_submap_pose, + mapping::proto::SubmapQuery::Response* const response) { + // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane + // in the global map frame. + const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid; + response->set_resolution(hybrid_grid.resolution()); + + // Compute a bounding box for the texture. + Eigen::Array2i min_index(INT_MAX, INT_MAX); + Eigen::Array2i max_index(INT_MIN, INT_MIN); + ExtractVoxelData( + hybrid_grid, + (global_submap_pose * Get(index)->local_pose().inverse()).cast(), + &min_index, &max_index); + + const int width = max_index.y() - min_index.y() + 1; + const int height = max_index.x() - min_index.x() + 1; + response->set_width(width); + response->set_height(height); + + AccumulatePixelData(width, height, min_index, max_index); + ComputePixelValues(width, height); + + common::FastGzipString(celldata_, response->mutable_cells()); + *response->mutable_slice_pose() = + transform::ToProto(global_submap_pose.inverse() * + transform::Rigid3d::Translation(Eigen::Vector3d( + max_index.x() * hybrid_grid.resolution(), + max_index.y() * hybrid_grid.resolution(), + global_submap_pose.translation().z()))); +} + +void Submaps::InsertLaserFan(const sensor::LaserFan3D& laser_fan) { + CHECK_LT(num_laser_fans_, std::numeric_limits::max()); + ++num_laser_fans_; + for (const int index : insertion_indices()) { + Submap* submap = submaps_[index].get(); + laser_fan_inserter_.Insert( + sensor::FilterLaserFanByMaxRange(laser_fan, + options_.high_resolution_max_range()), + &submap->high_resolution_hybrid_grid); + laser_fan_inserter_.Insert(laser_fan, &submap->low_resolution_hybrid_grid); + submap->end_laser_fan_index = num_laser_fans_; + } + ++num_laser_fans_in_last_submap_; + if (num_laser_fans_in_last_submap_ == options_.num_laser_fans()) { + AddSubmap(laser_fan.origin); + } +} + +const HybridGrid& Submaps::high_resolution_matching_grid() const { + return submaps_[matching_index()]->high_resolution_hybrid_grid; +} + +const HybridGrid& Submaps::low_resolution_matching_grid() const { + return submaps_[matching_index()]->low_resolution_hybrid_grid; +} + +void Submaps::AddTrajectoryNodeIndex(const int trajectory_node_index) { + for (int i = 0; i != size(); ++i) { + Submap& submap = *submaps_[i]; + if (submap.end_laser_fan_index == num_laser_fans_ && + submap.begin_laser_fan_index <= num_laser_fans_ - 1) { + submap.trajectory_node_indices.push_back(trajectory_node_index); + } + } +} + +void Submaps::AddSubmap(const Eigen::Vector3f& origin) { + if (size() > 1) { + Submap* submap = submaps_[size() - 2].get(); + CHECK(!submap->finished); + submap->finished = true; + } + submaps_.emplace_back(new Submap(options_.high_resolution(), + options_.low_resolution(), origin, + num_laser_fans_)); + LOG(INFO) << "Added submap " << size(); + num_laser_fans_in_last_submap_ = 0; +} + +void Submaps::AccumulatePixelData(const int width, const int height, + const Eigen::Array2i& min_index, + const Eigen::Array2i& max_index) { + accumulated_pixel_data_.clear(); + accumulated_pixel_data_.resize(width * height); + for (const Eigen::Array4i& voxel_index_and_probability : + voxel_indices_and_probabilities_) { + const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>(); + if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) { + // Out of bounds. This could happen because of floating point inaccuracy. + continue; + } + const int x = max_index.x() - pixel_index[0]; + const int y = max_index.y() - pixel_index[1]; + PixelData& pixel = accumulated_pixel_data_[x * width + y]; + ++pixel.count; + pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]); + pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]); + const float probability = + mapping::ValueToProbability(voxel_index_and_probability[3]); + pixel.probability_sum += probability; + pixel.max_probability = std::max(pixel.max_probability, probability); + } +} + +void Submaps::ExtractVoxelData(const HybridGrid& hybrid_grid, + const transform::Rigid3f& transform, + Eigen::Array2i* min_index, + Eigen::Array2i* max_index) { + voxel_indices_and_probabilities_.clear(); + const float resolution_inverse = 1. / hybrid_grid.resolution(); + + constexpr double kXrayObstructedCellProbabilityLimit = 0.501; + for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { + const uint16 probability_value = it.GetValue(); + const float probability = mapping::ValueToProbability(probability_value); + if (probability < kXrayObstructedCellProbabilityLimit) { + // We ignore non-obstructed cells. + continue; + } + + const Eigen::Vector3f cell_center_local = + hybrid_grid.GetCenterOfCell(it.GetCellIndex()); + const Eigen::Vector3f cell_center_global = transform * cell_center_local; + const Eigen::Array4i voxel_index_and_probability( + common::RoundToInt(cell_center_global.x() * resolution_inverse), + common::RoundToInt(cell_center_global.y() * resolution_inverse), + common::RoundToInt(cell_center_global.z() * resolution_inverse), + probability_value); + + voxel_indices_and_probabilities_.push_back(voxel_index_and_probability); + const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>(); + *min_index = min_index->cwiseMin(pixel_index); + *max_index = max_index->cwiseMax(pixel_index); + } +} + +void Submaps::ComputePixelValues(const int width, const int height) { + celldata_.resize(2 * width * height); + constexpr float kMinZDifference = 3.f; + constexpr float kFreeSpaceWeight = 0.15f; + auto it = celldata_.begin(); + for (size_t i = 0; i < accumulated_pixel_data_.size(); ++i) { + const PixelData& pixel = accumulated_pixel_data_.at(i); + // TODO(whess): Take into account submap rotation. + // TODO(whess): Document the approach and make it more independent from the + // chosen resolution. + const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0; + if (z_difference < kMinZDifference) { + *it = 0; // value + ++it; + *it = 0; // alpha + ++it; + continue; + } + const float free_space = std::max(z_difference - pixel.count, 0.f); + const float free_space_weight = kFreeSpaceWeight * free_space; + const float total_weight = pixel.count + free_space_weight; + const float free_space_probability = 1.f - pixel.max_probability; + const float average_probability = mapping::ClampProbability( + (pixel.probability_sum + free_space_probability * free_space_weight) / + total_weight); + const int delta = + 128 - mapping::ProbabilityToLogOddsInteger(average_probability); + const uint8 alpha = delta > 0 ? 0 : -delta; + const uint8 value = delta > 0 ? delta : 0; + *it = value; // value + ++it; + *it = (value || alpha) ? alpha : 1; // alpha + ++it; + } +} + +} // namespace mapping_3d +} // namespace cartographer diff --git a/cartographer/cartographer/mapping_3d/submaps.h b/cartographer/cartographer/mapping_3d/submaps.h new file mode 100644 index 0000000..d71553d --- /dev/null +++ b/cartographer/cartographer/mapping_3d/submaps.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_3D_SUBMAPS_H_ +#define CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ + +#include +#include +#include + +#include "Eigen/Geometry" +#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/submaps.h" +#include "cartographer/mapping_2d/laser_fan_inserter.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/mapping_3d/laser_fan_inserter.h" +#include "cartographer/mapping_3d/proto/submaps_options.pb.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping_3d { + +void InsertIntoProbabilityGrid( + const sensor::LaserFan3D& laser_fan_3d, const transform::Rigid3f& pose, + const float slice_z, const mapping_2d::LaserFanInserter& laser_fan_inserter, + mapping_2d::ProbabilityGrid* result); + +proto::SubmapsOptions CreateSubmapsOptions( + common::LuaParameterDictionary* parameter_dictionary); + +struct Submap : public mapping::Submap { + Submap(float high_resolution, float low_resolution, + const Eigen::Vector3f& origin, int begin_laser_fan_index); + + HybridGrid high_resolution_hybrid_grid; + HybridGrid low_resolution_hybrid_grid; + bool finished = false; + std::vector trajectory_node_indices; +}; + +// A container of Submaps. +class Submaps : public mapping::Submaps { + public: + explicit Submaps(const proto::SubmapsOptions& options); + + Submaps(const Submaps&) = delete; + Submaps& operator=(const Submaps&) = delete; + + const Submap* Get(int index) const override; + int size() const override; + void SubmapToProto( + int index, const std::vector& trajectory_nodes, + const transform::Rigid3d& global_submap_pose, + mapping::proto::SubmapQuery::Response* response) override; + + // Inserts 'laser_fan' into the Submap collection. + void InsertLaserFan(const sensor::LaserFan3D& laser_fan); + + // Returns the 'high_resolution' HybridGrid to be used for matching. + const HybridGrid& high_resolution_matching_grid() const; + + // Returns the 'low_resolution' HybridGrid to be used for matching. + const HybridGrid& low_resolution_matching_grid() const; + + // Adds a node to be used when visualizing the submap. + void AddTrajectoryNodeIndex(int trajectory_node_index); + + private: + struct PixelData { + int min_z = INT_MAX; + int max_z = INT_MIN; + int count = 0; + float probability_sum = 0.f; + float max_probability = 0.5f; + }; + + void AddSubmap(const Eigen::Vector3f& origin); + void AccumulatePixelData(const int width, const int height, + const Eigen::Array2i& min_index, + const Eigen::Array2i& max_index); + void ExtractVoxelData(const HybridGrid& hybrid_grid, + const transform::Rigid3f& transform, + Eigen::Array2i* min_index, Eigen::Array2i* max_index); + // Builds texture data containing interleaved value and alpha for the + // visualization from 'accumulated_pixel_data_' into 'celldata_'. + void ComputePixelValues(const int width, const int height); + + const proto::SubmapsOptions options_; + + std::vector> submaps_; + LaserFanInserter laser_fan_inserter_; + + // Number of LaserFans inserted. + int num_laser_fans_ = 0; + + // Number of LaserFans inserted since the last Submap was added. + int num_laser_fans_in_last_submap_ = 0; + + // The following members are used for visualization and kept around for + // performance reasons (mainly to avoid reallocations). + std::vector accumulated_pixel_data_; + string celldata_; + // The first three entries of this is are a cell_index and the last is the + // corresponding probability value. We batch them together like this to only + // have one vector and have better cache locality. + std::vector voxel_indices_and_probabilities_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ diff --git a/cartographer/cartographer/mapping_3d/translation_cost_function.h b/cartographer/cartographer/mapping_3d/translation_cost_function.h new file mode 100644 index 0000000..ffeca48 --- /dev/null +++ b/cartographer/cartographer/mapping_3d/translation_cost_function.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_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" + +namespace cartographer { +namespace mapping_3d { + +// Penalizes differences between velocity and change in position. +class TranslationCostFunction { + public: + TranslationCostFunction(const double scaling_factor, + const double delta_time_seconds) + : scaling_factor_(scaling_factor), + delta_time_seconds_(delta_time_seconds) {} + + TranslationCostFunction(const TranslationCostFunction&) = delete; + TranslationCostFunction& operator=(const TranslationCostFunction&) = delete; + + template + bool operator()(const T* const start_translation, + const T* const end_translation, const T* const velocity, + T* residual) const { + const T delta_x = end_translation[0] - start_translation[0]; + const T delta_y = end_translation[1] - start_translation[1]; + const T delta_z = end_translation[2] - start_translation[2]; + + residual[0] = + scaling_factor_ * (delta_x - velocity[0] * delta_time_seconds_); + residual[1] = + scaling_factor_ * (delta_y - velocity[1] * delta_time_seconds_); + residual[2] = + scaling_factor_ * (delta_z - velocity[2] * delta_time_seconds_); + return true; + } + + private: + const double scaling_factor_; + const double delta_time_seconds_; +}; + +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ diff --git a/cartographer/cartographer/proto/CMakeLists.txt b/cartographer/cartographer/proto/CMakeLists.txt new file mode 100644 index 0000000..b86f7d8 --- /dev/null +++ b/cartographer/cartographer/proto/CMakeLists.txt @@ -0,0 +1,20 @@ +# 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(proto_trajectory + SRCS + trajectory.proto + DEPENDS + transform_proto_transform +) diff --git a/cartographer/cartographer/proto/trajectory.proto b/cartographer/cartographer/proto/trajectory.proto new file mode 100644 index 0000000..3a6d861 --- /dev/null +++ b/cartographer/cartographer/proto/trajectory.proto @@ -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. + +syntax = "proto2"; + +import "cartographer/transform/proto/transform.proto"; + +package cartographer.proto; + +option java_outer_classname = "TrajectoryOuterClass"; + +message Trajectory { + // NEXT_ID: 7 + message Node { + optional int64 timestamp = 1; + // 2D Pose in map frame. + optional transform.proto.Rigid2d pose_2d = 2; + // Transform from tracking to map frame. + optional transform.proto.Rigid3d pose = 5; + } + + // Time-ordered sequence of Nodes. + repeated Node node = 1; +} diff --git a/cartographer/cartographer/sensor/CMakeLists.txt b/cartographer/cartographer/sensor/CMakeLists.txt new file mode 100644 index 0000000..d48880e --- /dev/null +++ b/cartographer/cartographer/sensor/CMakeLists.txt @@ -0,0 +1,122 @@ +# 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(sensor_compressed_point_cloud + USES_EIGEN + SRCS + compressed_point_cloud.cc + HDRS + compressed_point_cloud.h + DEPENDS + common_math + common_port + mapping_3d_hybrid_grid + sensor_point_cloud + sensor_proto_sensor +) + +google_library(sensor_configuration + USES_CERES + USES_EIGEN + SRCS + configuration.cc + HDRS + configuration.h + DEPENDS + common_lua_parameter_dictionary + sensor_proto_configuration + transform_proto_transform + transform_rigid_transform + transform_transform +) + +google_library(sensor_laser + SRCS + laser.cc + HDRS + laser.h + DEPENDS + common_port + sensor_compressed_point_cloud + sensor_point_cloud + sensor_proto_sensor + transform_transform +) + +google_library(sensor_point_cloud + USES_CERES + USES_EIGEN + SRCS + point_cloud.cc + HDRS + point_cloud.h + DEPENDS + sensor_proto_sensor + transform_rigid_transform + transform_transform +) + +google_library(sensor_sensor_packet_period_histogram_builder + USES_CERES + SRCS + sensor_packet_period_histogram_builder.cc + HDRS + sensor_packet_period_histogram_builder.h + DEPENDS + common_histogram + common_port +) + +google_library(sensor_voxel_filter + SRCS + voxel_filter.cc + HDRS + voxel_filter.h + DEPENDS + common_lua_parameter_dictionary + common_math + mapping_3d_hybrid_grid + sensor_point_cloud + sensor_proto_adaptive_voxel_filter_options +) + +google_test(sensor_compressed_point_cloud_test + SRCS + compressed_point_cloud_test.cc + DEPENDS + sensor_compressed_point_cloud +) + +google_test(sensor_laser_test + SRCS + laser_test.cc + DEPENDS + sensor_laser +) + +google_test(sensor_point_cloud_test + SRCS + point_cloud_test.cc + DEPENDS + sensor_point_cloud +) + +google_test(sensor_voxel_filter_test + SRCS + voxel_filter_test.cc + DEPENDS + sensor_voxel_filter +) diff --git a/cartographer/cartographer/sensor/compressed_point_cloud.cc b/cartographer/cartographer/sensor/compressed_point_cloud.cc new file mode 100644 index 0000000..2d67d72 --- /dev/null +++ b/cartographer/cartographer/sensor/compressed_point_cloud.cc @@ -0,0 +1,203 @@ +/* + * 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/sensor/compressed_point_cloud.h" + +#include + +#include "cartographer/common/math.h" +#include "cartographer/mapping_3d/hybrid_grid.h" + +namespace cartographer { +namespace sensor { + +namespace { + +// Points are encoded on a fixed grid with a grid spacing of 'kPrecision' with +// integers. Points are organized in blocks, where each point is encoded +// relative to the block's origin in an int32 with 'kBitsPerCoordinate' bits per +// coordinate. +constexpr float kPrecision = 0.001f; // in meters. +constexpr int kBitsPerCoordinate = 10; +constexpr int kCoordinateMask = (1 << kBitsPerCoordinate) - 1; +constexpr int kMaxBitsPerDirection = 23; + +// Compressed point cloud into a vector and optionally returns mapping from +// compressed indices to original indices. +std::vector Compress(const PointCloud& point_cloud, + std::vector* new_to_old = nullptr) { + // Distribute points into blocks. + struct RasterPoint { + Eigen::Array3i point; + int index; + }; + using Blocks = mapping_3d::HybridGridBase>; + Blocks blocks(kPrecision, Eigen::Vector3f::Zero()); + int num_blocks = 0; + CHECK_LE(point_cloud.size(), std::numeric_limits::max()); + for (int point_index = 0; point_index < static_cast(point_cloud.size()); + ++point_index) { + const Eigen::Vector3f& point = point_cloud[point_index]; + CHECK_LT(point.cwiseAbs().maxCoeff() / kPrecision, + 1 << kMaxBitsPerDirection) + << "Point out of bounds: " << point; + Eigen::Array3i raster_point; + Eigen::Array3i block_coordinate; + for (int i = 0; i < 3; ++i) { + raster_point[i] = common::RoundToInt(point[i] / kPrecision); + block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate; + raster_point[i] &= kCoordinateMask; + } + auto* const block = blocks.mutable_value(block_coordinate); + num_blocks += block->empty(); + block->push_back({raster_point, point_index}); + } + + if (new_to_old != nullptr) { + new_to_old->clear(); + new_to_old->reserve(point_cloud.size()); + } + + // Encode blocks. + std::vector result; + result.reserve(4 * num_blocks + point_cloud.size()); + for (Blocks::Iterator it(blocks); !it.Done(); it.Next(), --num_blocks) { + const auto& raster_points = it.GetValue(); + CHECK_LE(raster_points.size(), std::numeric_limits::max()); + result.push_back(raster_points.size()); + const Eigen::Array3i block_coordinate = it.GetCellIndex(); + result.push_back(block_coordinate.x()); + result.push_back(block_coordinate.y()); + result.push_back(block_coordinate.z()); + for (const RasterPoint& raster_point : raster_points) { + result.push_back((((raster_point.point.z() << kBitsPerCoordinate) + + raster_point.point.y()) + << kBitsPerCoordinate) + + raster_point.point.x()); + if (new_to_old != nullptr) { + new_to_old->push_back(raster_point.index); + } + } + } + CHECK_EQ(num_blocks, 0); + return result; +} + +} // namespace + +CompressedPointCloud::ConstIterator::ConstIterator( + const CompressedPointCloud* compressed_point_cloud) + : compressed_point_cloud_(compressed_point_cloud), + remaining_points_(compressed_point_cloud->num_points_), + remaining_points_in_current_block_(0), + input_(compressed_point_cloud->point_data_.begin()) { + if (remaining_points_ > 0) { + ReadNextPoint(); + } +} + +CompressedPointCloud::ConstIterator +CompressedPointCloud::ConstIterator::EndIterator( + const CompressedPointCloud* compressed_point_cloud) { + ConstIterator end_iterator(compressed_point_cloud); + end_iterator.remaining_points_ = 0; + return end_iterator; +} + +Eigen::Vector3f CompressedPointCloud::ConstIterator::operator*() const { + CHECK_GT(remaining_points_, 0); + return current_point_; +} + +CompressedPointCloud::ConstIterator& CompressedPointCloud::ConstIterator:: +operator++() { + --remaining_points_; + if (remaining_points_ > 0) { + ReadNextPoint(); + } + return *this; +} + +bool CompressedPointCloud::ConstIterator::operator!=( + const ConstIterator& it) const { + CHECK(compressed_point_cloud_ == it.compressed_point_cloud_); + return remaining_points_ != it.remaining_points_; +} + +void CompressedPointCloud::ConstIterator::ReadNextPoint() { + if (remaining_points_in_current_block_ == 0) { + remaining_points_in_current_block_ = *input_++; + for (int i = 0; i < 3; ++i) { + current_block_coordinates_[i] = *input_++ << kBitsPerCoordinate; + } + } + --remaining_points_in_current_block_; + const int point = *input_++; + constexpr int kMask = (1 << kBitsPerCoordinate) - 1; + current_point_[0] = + (current_block_coordinates_[0] + (point & kMask)) * kPrecision; + current_point_[1] = (current_block_coordinates_[1] + + ((point >> kBitsPerCoordinate) & kMask)) * + kPrecision; + current_point_[2] = + (current_block_coordinates_[2] + (point >> (2 * kBitsPerCoordinate))) * + kPrecision; +} + +CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud) + : point_data_(Compress(point_cloud)), num_points_(point_cloud.size()) {} + +CompressedPointCloud::CompressedPointCloud(const std::vector& point_data, + size_t num_points) + : point_data_(point_data), num_points_(num_points) {} + +CompressedPointCloud CompressedPointCloud::CompressAndReturnOrder( + const PointCloud& point_cloud, std::vector* new_to_old) { + return CompressedPointCloud(Compress(point_cloud, new_to_old), + point_cloud.size()); +} + +bool CompressedPointCloud::empty() const { return num_points_ == 0; } + +size_t CompressedPointCloud::size() const { return num_points_; } + +CompressedPointCloud::ConstIterator CompressedPointCloud::begin() const { + return ConstIterator(this); +} + +CompressedPointCloud::ConstIterator CompressedPointCloud::end() const { + return ConstIterator::EndIterator(this); +} + +PointCloud CompressedPointCloud::Decompress() const { + PointCloud decompressed; + for (const Eigen::Vector3f& point : *this) { + decompressed.push_back(point); + } + return decompressed; +} + +proto::CompressedPointCloud CompressedPointCloud::ToProto() const { + proto::CompressedPointCloud result; + result.set_num_points(num_points_); + for (const int32 data : point_data_) { + result.add_point_data(data); + } + return result; +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/compressed_point_cloud.h b/cartographer/cartographer/sensor/compressed_point_cloud.h new file mode 100644 index 0000000..eb04b23 --- /dev/null +++ b/cartographer/cartographer/sensor/compressed_point_cloud.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_SENSOR_COMPRESSED_POINT_CLOUD_H_ +#define CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ + +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/port.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/proto/sensor.pb.h" + +namespace cartographer { +namespace sensor { + +// A compressed representation of a point cloud consisting of a collection of +// points (Vector3f). +// Internally, points are grouped by blocks. Each block encodes a bit of meta +// data (number of points in block, coordinates of the block) and encodes each +// point with a fixed bit rate in relation to the block. +class CompressedPointCloud { + public: + class ConstIterator; + + CompressedPointCloud() : num_points_(0) {} + explicit CompressedPointCloud(const PointCloud& point_cloud); + + // Returns a compressed point cloud and further returns a mapping 'new_to_old' + // from the compressed indices to the original indices, i.e., conceptually + // compressed[i] = point_cloud[new_to_old[i]]. + static CompressedPointCloud CompressAndReturnOrder( + const PointCloud& point_cloud, std::vector* new_to_old); + + // Returns decompressed point cloud. + PointCloud Decompress() const; + + bool empty() const; + size_t size() const; + ConstIterator begin() const; + ConstIterator end() const; + + proto::CompressedPointCloud ToProto() const; + + private: + CompressedPointCloud(const std::vector& point_data, size_t num_points); + + const std::vector point_data_; + const size_t num_points_; +}; + +// Forward iterator for compressed point clouds. +class CompressedPointCloud::ConstIterator + : public std::iterator { + public: + // Creates begin iterator. + explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud); + + // Creates end iterator. + static ConstIterator EndIterator( + const CompressedPointCloud* compressed_point_cloud); + + Eigen::Vector3f operator*() const; + ConstIterator& operator++(); + bool operator!=(const ConstIterator& it) const; + + private: + // Reads next point from buffer. Also handles reading the meta data of the + // next block, if the current block is depleted. + void ReadNextPoint(); + + const CompressedPointCloud* compressed_point_cloud_; + size_t remaining_points_; + int32 remaining_points_in_current_block_; + Eigen::Vector3f current_point_; + Eigen::Vector3i current_block_coordinates_; + std::vector::const_iterator input_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ diff --git a/cartographer/cartographer/sensor/compressed_point_cloud_test.cc b/cartographer/cartographer/sensor/compressed_point_cloud_test.cc new file mode 100644 index 0000000..301be2d --- /dev/null +++ b/cartographer/cartographer/sensor/compressed_point_cloud_test.cc @@ -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. + */ + +#include "cartographer/sensor/compressed_point_cloud.h" + +#include "gmock/gmock.h" + +namespace Eigen { + +// Prints Vector3f in a readable format in matcher ApproximatelyEquals when +// failing a test. Without this function, the output is formated as hexadecimal +// 8 bit numbers. +void PrintTo(const Vector3f& x, std::ostream* os) { + *os << "(" << x[0] << ", " << x[1] << ", " << x[2] << ")"; +} + +} // namespace Eigen + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::Contains; +using ::testing::FloatNear; +using ::testing::PrintToString; + +constexpr float kPrecision = 0.001f; + +// Matcher for 3-d vectors w.r.t. to the target precision. +MATCHER_P(ApproximatelyEquals, expected, + string("is equal to ") + PrintToString(expected)) { + return (arg - expected).isZero(kPrecision); +} + +// Helper function to test the mapping of a single point. Includes test for +// recompressing the same point again. +void TestPoint(const Eigen::Vector3f& p) { + CompressedPointCloud compressed({p}); + EXPECT_EQ(1, compressed.size()); + EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p)); + CompressedPointCloud recompressed({*compressed.begin()}); + EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p)); +} + +TEST(CompressPointCloudTest, CompressesPointsCorrectly) { + TestPoint(Eigen::Vector3f(8000.f, 7500.f, 5000.f)); + TestPoint(Eigen::Vector3f(1000.f, 2000.f, 3000.f)); + TestPoint(Eigen::Vector3f(100.f, 200.f, 300.f)); + TestPoint(Eigen::Vector3f(10.f, 20.f, 30.f)); + TestPoint(Eigen::Vector3f(-0.00049f, -0.0005f, -0.0015f)); + TestPoint(Eigen::Vector3f(0.05119f, 0.0512f, 0.05121)); + TestPoint(Eigen::Vector3f(-0.05119f, -0.0512f, -0.05121)); + TestPoint(Eigen::Vector3f(0.8405f, 0.84f, 0.8396f)); + TestPoint(Eigen::Vector3f(0.8395f, 0.8394f, 0.8393f)); + TestPoint(Eigen::Vector3f(0.839f, 0.8391f, 0.8392f)); + TestPoint(Eigen::Vector3f(0.8389f, 0.8388f, 0.83985f)); +} + +TEST(CompressPointCloudTest, Compresses) { + const CompressedPointCloud compressed({Eigen::Vector3f(0.838f, 0, 0), + Eigen::Vector3f(0.839f, 0, 0), + Eigen::Vector3f(0.840f, 0, 0)}); + EXPECT_FALSE(compressed.empty()); + EXPECT_EQ(3, compressed.size()); + const PointCloud decompressed = compressed.Decompress(); + EXPECT_EQ(3, decompressed.size()); + EXPECT_THAT(decompressed, + Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0)))); + EXPECT_THAT(decompressed, + Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0)))); + EXPECT_THAT(decompressed, + Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0)))); +} + +TEST(CompressPointCloudTest, CompressesEmptyPointCloud) { + CompressedPointCloud compressed; + EXPECT_TRUE(compressed.empty()); + EXPECT_EQ(0, compressed.size()); +} + +// Test for gaps. +// Produces a series of points densly packed along the x axis, compresses these +// points (twice), and tests, whether there are gaps between two consecutive +// points. +TEST(CompressPointCloudTest, CompressesNoGaps) { + PointCloud point_cloud; + for (int i = 0; i < 3000; ++i) { + point_cloud.push_back(Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0)); + } + const CompressedPointCloud compressed(point_cloud); + const PointCloud decompressed = compressed.Decompress(); + const CompressedPointCloud recompressed(decompressed); + EXPECT_EQ(decompressed.size(), recompressed.size()); + + std::vector x_coord; + for (const auto& p : compressed) { + x_coord.push_back(p[0]); + } + std::sort(x_coord.begin(), x_coord.end()); + for (size_t i = 1; i < x_coord.size(); ++i) { + EXPECT_THAT(std::abs(x_coord[i] - x_coord[i - 1]), + FloatNear(kPrecision, 1e-7f)); + } +} + +TEST(CompressPointCloudTest, CompressesAndReturnsOrder) { + PointCloud point_cloud = { + Eigen::Vector3f(1.f, 0.f, 0.f), Eigen::Vector3f(-10.f, 0.f, 0.f), + Eigen::Vector3f(1.f, 0.f, 0.f), Eigen::Vector3f(-10.f, 0.f, 0.f)}; + std::vector new_to_old; + CompressedPointCloud compressed = + CompressedPointCloud::CompressAndReturnOrder(point_cloud, &new_to_old); + EXPECT_EQ(point_cloud.size(), new_to_old.size()); + int i = 0; + for (const Eigen::Vector3f& point : compressed) { + EXPECT_THAT(point, ApproximatelyEquals(point_cloud[new_to_old[i]])); + ++i; + } +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/configuration.cc b/cartographer/cartographer/sensor/configuration.cc new file mode 100644 index 0000000..d88743c --- /dev/null +++ b/cartographer/cartographer/sensor/configuration.cc @@ -0,0 +1,73 @@ +/* + * 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/sensor/configuration.h" + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/sensor/proto/configuration.pb.h" +#include "cartographer/transform/proto/transform.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +proto::Configuration::Sensor CreateSensorConfiguration( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::Configuration::Sensor sensor; + sensor.set_frame_id(parameter_dictionary->GetString("frame_id")); + *sensor.mutable_transform() = transform::ToProto(transform::FromDictionary( + parameter_dictionary->GetDictionary("transform").get())); + return sensor; +} + +proto::Configuration CreateConfiguration( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::Configuration configuration; + for (auto& sensor_parameter_dictionary : + parameter_dictionary->GetArrayValuesAsDictionaries()) { + *configuration.add_sensor() = + CreateSensorConfiguration(sensor_parameter_dictionary.get()); + } + return configuration; +} + +bool IsEnabled(const string& frame_id, + const sensor::proto::Configuration& sensor_configuration) { + for (const auto& sensor : sensor_configuration.sensor()) { + if (sensor.frame_id() == frame_id) { + return true; + } + } + return false; +} + +transform::Rigid3d GetTransformToTracking( + const string& frame_id, + const sensor::proto::Configuration& sensor_configuration) { + for (const auto& sensor : sensor_configuration.sensor()) { + if (sensor.frame_id() == frame_id) { + return transform::ToRigid3(sensor.transform()); + } + } + LOG(FATAL) << "No configuration found for sensor with frame ID '" << frame_id + << "'."; +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/configuration.h b/cartographer/cartographer/sensor/configuration.h new file mode 100644 index 0000000..c632371 --- /dev/null +++ b/cartographer/cartographer/sensor/configuration.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_SENSOR_CONFIGURATION_H_ +#define CARTOGRAPHER_SENSOR_CONFIGURATION_H_ + +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/sensor/proto/configuration.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace sensor { + +// Creates the configuration for a singular sensor from 'parameter_dictionary'. +proto::Configuration::Sensor CreateSensorConfiguration( + common::LuaParameterDictionary* parameter_dictionary); + +// Creates the mapping from frame_id to Sensors defined in +// 'parameter_dictionary'. +proto::Configuration CreateConfiguration( + common::LuaParameterDictionary* parameter_dictionary); + +// Returns true if 'frame_id' is mentioned in 'sensor_configuration'. +bool IsEnabled(const string& frame_id, + const sensor::proto::Configuration& sensor_configuration); + +// Returns the transform which takes data from the sensor frame to the +// tracking frame. +transform::Rigid3d GetTransformToTracking( + const string& frame_id, + const sensor::proto::Configuration& sensor_configuration); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_CONFIGURATION_H_ diff --git a/cartographer/cartographer/sensor/laser.cc b/cartographer/cartographer/sensor/laser.cc new file mode 100644 index 0000000..400e501 --- /dev/null +++ b/cartographer/cartographer/sensor/laser.cc @@ -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. + */ + +#include "cartographer/sensor/laser.h" + +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +namespace { + +// Reorders reflectivities according to index mapping. +std::vector ReorderReflectivities( + const std::vector& reflectivities, + const std::vector& new_to_old) { + std::vector reordered(reflectivities.size()); + for (size_t i = 0; i < reordered.size(); ++i) { + reordered[i] = reflectivities[new_to_old[i]]; + } + return reordered; +} + +} // namespace + +LaserFan ToLaserFan(const proto::LaserScan& proto, const float min_range, + const float max_range, + const float missing_echo_ray_length) { + CHECK_GE(min_range, 0.f); + CHECK_GT(proto.angle_increment(), 0.f); + CHECK_GT(proto.angle_max(), proto.angle_min()); + LaserFan laser_fan = {Eigen::Vector2f::Zero(), {}, {}}; + float angle = proto.angle_min(); + for (const auto& range : proto.range()) { + if (range.value_size() > 0) { + const float first_echo = range.value(0); + if (!std::isnan(first_echo) && first_echo >= min_range) { + if (first_echo <= max_range) { + laser_fan.point_cloud.push_back(Eigen::Rotation2Df(angle) * + Eigen::Vector2f(first_echo, 0.f)); + } else { + laser_fan.missing_echo_point_cloud.push_back( + Eigen::Rotation2Df(angle) * + Eigen::Vector2f(missing_echo_ray_length, 0.f)); + } + } + } + angle += proto.angle_increment(); + } + return laser_fan; +} + +LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan, + const Eigen::Vector3f& min, + const Eigen::Vector3f& max) { + return LaserFan{laser_fan.origin.head<2>(), + ProjectToPointCloud2D(Crop(laser_fan.returns, min, max)), + ProjectToPointCloud2D(Crop(laser_fan.misses, min, max))}; +} + +LaserFan TransformLaserFan(const LaserFan& laser_fan, + const transform::Rigid2f& transform) { + return LaserFan{ + transform * laser_fan.origin, + TransformPointCloud2D(laser_fan.point_cloud, transform), + TransformPointCloud2D(laser_fan.missing_echo_point_cloud, transform)}; +} + +LaserFan3D ToLaserFan3D(const LaserFan& laser_fan) { + return LaserFan3D{ + Eigen::Vector3f(laser_fan.origin.x(), laser_fan.origin.y(), 0.), + ToPointCloud(laser_fan.point_cloud), + ToPointCloud(laser_fan.missing_echo_point_cloud)}; +} + +LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan) { + return LaserFan3D{compressed_laser_fan.origin, + compressed_laser_fan.returns.Decompress(), + compressed_laser_fan.misses.Decompress(), + compressed_laser_fan.reflectivities}; +} + +CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) { + std::vector new_to_old; + CompressedPointCloud compressed_returns = + CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns, + &new_to_old); + return CompressedLaserFan3D{ + laser_fan.origin, std::move(compressed_returns), + CompressedPointCloud(laser_fan.misses), + ReorderReflectivities(laser_fan.reflectivities, new_to_old)}; +} + +LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan, + const transform::Rigid3f& transform) { + return LaserFan3D{ + transform * laser_fan.origin, + TransformPointCloud(laser_fan.returns, transform), + TransformPointCloud(laser_fan.misses, transform), + laser_fan.reflectivities, + }; +} + +proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) { + proto::LaserFan3D proto; + *proto.mutable_origin() = transform::ToProto(laser_fan.origin); + *proto.mutable_point_cloud() = ToProto(laser_fan.returns); + *proto.mutable_missing_echo_point_cloud() = ToProto(laser_fan.misses); + std::copy(laser_fan.reflectivities.begin(), laser_fan.reflectivities.end(), + RepeatedFieldBackInserter(proto.mutable_reflectivity())); + return proto; +} + +LaserFan3D FromProto(const proto::LaserFan3D& proto) { + auto laser_fan_3d = LaserFan3D{ + transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()), + ToPointCloud(proto.missing_echo_point_cloud()), + }; + std::copy(proto.reflectivity().begin(), proto.reflectivity().end(), + std::back_inserter(laser_fan_3d.reflectivities)); + return laser_fan_3d; +} + +LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, + const float max_range) { + LaserFan3D result{laser_fan.origin, {}, {}, {}}; + for (const Eigen::Vector3f& return_ : laser_fan.returns) { + if ((return_ - laser_fan.origin).norm() <= max_range) { + result.returns.push_back(return_); + } + } + return result; +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/laser.h b/cartographer/cartographer/sensor/laser.h new file mode 100644 index 0000000..453ed39 --- /dev/null +++ b/cartographer/cartographer/sensor/laser.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_SENSOR_LASER_H_ +#define CARTOGRAPHER_SENSOR_LASER_H_ + +#include "cartographer/common/port.h" +#include "cartographer/sensor/compressed_point_cloud.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/proto/sensor.pb.h" + +namespace cartographer { +namespace sensor { + +// Builds a LaserFan from 'proto' and separates any beams with ranges outside +// the range ['min_range', 'max_range']. Beams beyond 'max_range' inserted into +// the 'missing_echo_point_cloud' with length 'missing_echo_ray_length'. The +// points in both clouds are stored in scan order. +struct LaserFan { + Eigen::Vector2f origin; + PointCloud2D point_cloud; + PointCloud2D missing_echo_point_cloud; +}; +LaserFan ToLaserFan(const proto::LaserScan& proto, float min_range, + float max_range, float missing_echo_ray_length); + +// Transforms 'laser_fan' according to 'transform'. +LaserFan TransformLaserFan(const LaserFan& laser_fan, + const transform::Rigid2f& transform); + +// A 3D variant of LaserFan. Rays begin at 'origin'. 'returns' are the points +// where laser returns were detected. 'misses' are points in the direction of +// rays for which no return was detected, and were inserted at a configured +// distance. It is assumed that between the 'origin' and 'misses' is free space. +struct LaserFan3D { + Eigen::Vector3f origin; + PointCloud returns; + PointCloud misses; + + // Reflectivity value of returns. + std::vector reflectivities; +}; + +// Like LaserFan3D but with compressed point clouds. The point order changes +// when converting from LaserFan3D. +struct CompressedLaserFan3D { + Eigen::Vector3f origin; + CompressedPointCloud returns; + CompressedPointCloud misses; + + // Reflectivity value of returns. + std::vector reflectivities; +}; + +LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan); + +CompressedLaserFan3D Compress(const LaserFan3D& laser_fan); + +// Converts 3D 'laser_fan' to a proto::LaserFan3D. +proto::LaserFan3D ToProto(const LaserFan3D& laser_fan); + +// Converts 'proto' to a LaserFan3D. +LaserFan3D FromProto(const proto::LaserFan3D& proto); + +LaserFan3D ToLaserFan3D(const LaserFan& laser_fan); + +LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan, + const transform::Rigid3f& transform); + +// Projects 'laser_fan' into 2D and crops it according to the cuboid defined by +// 'min' and 'max'. +LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan, + const Eigen::Vector3f& min, + const Eigen::Vector3f& max); + +// Filter a 'laser_fan', retaining only the returns that have no more than +// 'max_range' distance from the laser origin. Removes misses and reflectivity +// information. +LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, + float max_range); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_LASER_H_ diff --git a/cartographer/cartographer/sensor/laser_test.cc b/cartographer/cartographer/sensor/laser_test.cc new file mode 100644 index 0000000..6bf210c --- /dev/null +++ b/cartographer/cartographer/sensor/laser_test.cc @@ -0,0 +1,116 @@ +/* + * 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/sensor/laser.h" + +#include +#include + +#include "gmock/gmock.h" + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::Contains; +using ::testing::PrintToString; + +TEST(ProjectorTest, ToLaserFan) { + proto::LaserScan laser_scan; + for (int i = 0; i < 8; ++i) { + laser_scan.add_range()->add_value(1.f); + } + laser_scan.set_angle_min(0.f); + laser_scan.set_angle_max(8.f * static_cast(M_PI_4)); + laser_scan.set_angle_increment(static_cast(M_PI_4)); + + const LaserFan fan = ToLaserFan(laser_scan, 0.f, 10.f, 1.f); + EXPECT_TRUE(fan.point_cloud[0].isApprox(Eigen::Vector2f(1.f, 0.f), 1e-6)); + EXPECT_TRUE(fan.point_cloud[1].isApprox( + Eigen::Vector2f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6)); + EXPECT_TRUE(fan.point_cloud[2].isApprox(Eigen::Vector2f(0.f, 1.f), 1e-6)); + EXPECT_TRUE(fan.point_cloud[3].isApprox( + Eigen::Vector2f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6)); + EXPECT_TRUE(fan.point_cloud[4].isApprox(Eigen::Vector2f(-1.f, 0.f), 1e-6)); + EXPECT_TRUE(fan.point_cloud[5].isApprox( + Eigen::Vector2f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f)), 1e-6)); + EXPECT_TRUE(fan.point_cloud[6].isApprox(Eigen::Vector2f(0.f, -1.f), 1e-6)); + EXPECT_TRUE(fan.point_cloud[7].isApprox( + Eigen::Vector2f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f)), 1e-6)); +} + +TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) { + proto::LaserScan laser_scan; + laser_scan.add_range()->add_value(1.f); + laser_scan.add_range()->add_value(std::numeric_limits::infinity()); + laser_scan.add_range()->add_value(2.f); + laser_scan.add_range()->add_value(std::numeric_limits::quiet_NaN()); + laser_scan.add_range()->add_value(3.f); + laser_scan.set_angle_min(0.f); + laser_scan.set_angle_max(3.f * static_cast(M_PI_4)); + laser_scan.set_angle_increment(static_cast(M_PI_4)); + + const LaserFan fan = ToLaserFan(laser_scan, 2.f, 10.f, 1.f); + ASSERT_EQ(2, fan.point_cloud.size()); + EXPECT_TRUE(fan.point_cloud[0].isApprox(Eigen::Vector2f(0.f, 2.f), 1e-6)); + EXPECT_TRUE(fan.point_cloud[1].isApprox(Eigen::Vector2f(-3.f, 0.f), 1e-6)); + + ASSERT_EQ(1, fan.missing_echo_point_cloud.size()); + EXPECT_TRUE(fan.missing_echo_point_cloud[0].isApprox( + Eigen::Vector2f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6)); +} + +// Custom matcher for pair entries. +MATCHER_P(PairApproximatelyEquals, expected, + string("is equal to ") + PrintToString(expected)) { + return (arg.first - expected.first).isZero(0.001f) && + arg.second == expected.second; +} + +TEST(LaserTest, Compression) { + LaserFan3D fan = {Eigen::Vector3f(1, 1, 1), + {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), + Eigen::Vector3f(0, 1, 2)}, + {Eigen::Vector3f(7, 8, 9)}, + {1, 2, 3}}; + LaserFan3D actual = Decompress(Compress(fan)); + EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6)); + EXPECT_EQ(3, actual.returns.size()); + EXPECT_EQ(1, actual.misses.size()); + EXPECT_EQ(actual.returns.size(), actual.reflectivities.size()); + EXPECT_TRUE(actual.misses[0].isApprox(Eigen::Vector3f(7, 8, 9), 0.001f)); + + // Returns and their corresponding reflectivities will be reordered, so we + // pair them up into a vector, and compare in an unordered manner. + std::vector> pairs; + for (size_t i = 0; i < actual.returns.size(); ++i) { + pairs.emplace_back(actual.returns[i], actual.reflectivities[i]); + } + EXPECT_EQ(3, pairs.size()); + EXPECT_THAT(pairs, + Contains(PairApproximatelyEquals(std::pair( + Eigen::Vector3f(0, 1, 2), 1)))); + EXPECT_THAT(pairs, + Contains(PairApproximatelyEquals(std::pair( + Eigen::Vector3f(0, 1, 2), 3)))); + EXPECT_THAT(pairs, + Contains(PairApproximatelyEquals(std::pair( + Eigen::Vector3f(4, 5, 6), 2)))); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/point_cloud.cc b/cartographer/cartographer/sensor/point_cloud.cc new file mode 100644 index 0000000..b937dac --- /dev/null +++ b/cartographer/cartographer/sensor/point_cloud.cc @@ -0,0 +1,101 @@ +/* + * 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/sensor/point_cloud.h" + +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace sensor { + +namespace { + +template +PointCloudType Transform(const PointCloudType& point_cloud, + const TransformType& transform) { + PointCloudType result; + result.reserve(point_cloud.size()); + for (const auto& point : point_cloud) { + result.emplace_back(transform * point); + } + return result; +} + +} // namespace + +PointCloud TransformPointCloud(const PointCloud& point_cloud, + const transform::Rigid3f& transform) { + return Transform(point_cloud, transform); +} + +PointCloud2D TransformPointCloud2D(const PointCloud2D& point_cloud_2d, + const transform::Rigid2f& transform) { + return Transform(point_cloud_2d, transform); +} + +PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d) { + sensor::PointCloud point_cloud; + point_cloud.reserve(point_cloud_2d.size()); + for (const auto& point : point_cloud_2d) { + point_cloud.emplace_back(point.x(), point.y(), 0.f); + } + return point_cloud; +} + +PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud) { + sensor::PointCloud2D point_cloud_2d; + point_cloud_2d.reserve(point_cloud.size()); + for (const auto& point : point_cloud) { + point_cloud_2d.emplace_back(point.x(), point.y()); + } + return point_cloud_2d; +} + +PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min, + const Eigen::Vector3f& max) { + PointCloud cropped_point_cloud; + for (const auto& point : point_cloud) { + if (min.x() <= point.x() && point.x() <= max.x() && min.y() <= point.y() && + point.y() <= max.y() && min.z() <= point.z() && point.z() <= max.z()) { + cropped_point_cloud.push_back(point); + } + } + return cropped_point_cloud; +} + +proto::PointCloud ToProto(const PointCloud& point_cloud) { + proto::PointCloud proto; + for (const auto& point : point_cloud) { + proto.add_x(point.x()); + proto.add_y(point.y()); + proto.add_z(point.z()); + } + return proto; +} + +PointCloud ToPointCloud(const proto::PointCloud& proto) { + PointCloud point_cloud; + const int size = std::min({proto.x_size(), proto.y_size(), proto.z_size()}); + point_cloud.reserve(size); + for (int i = 0; i != size; ++i) { + point_cloud.emplace_back(proto.x(i), proto.y(i), proto.z(i)); + } + return point_cloud; +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/point_cloud.h b/cartographer/cartographer/sensor/point_cloud.h new file mode 100644 index 0000000..8b8705b --- /dev/null +++ b/cartographer/cartographer/sensor/point_cloud.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_SENSOR_POINT_CLOUD_H_ +#define CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +typedef std::vector PointCloud; +typedef std::vector PointCloud2D; + +// Transforms 'point_cloud' according to 'transform'. +PointCloud TransformPointCloud(const PointCloud& point_cloud, + const transform::Rigid3f& transform); + +// Transforms 'point_cloud_2d' according to 'transform'. +PointCloud2D TransformPointCloud2D(const PointCloud2D& point_cloud_2d, + const transform::Rigid2f& transform); + +// Converts 'point_cloud_2d' to a 3D point cloud. +PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d); + +// Converts 'point_cloud' to a 2D point cloud by removing the z component. +PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud); + +// Returns a new point cloud without points that fall outside the axis-aligned +// cuboid defined by 'min' and 'max'. +PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min, + const Eigen::Vector3f& max); + +// Converts 'point_cloud' to a proto::PointCloud. +proto::PointCloud ToProto(const PointCloud& point_cloud); + +// Converts 'proto' to a PointCloud. +PointCloud ToPointCloud(const proto::PointCloud& proto); + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ diff --git a/cartographer/cartographer/sensor/point_cloud_test.cc b/cartographer/cartographer/sensor/point_cloud_test.cc new file mode 100644 index 0000000..0b928a4 --- /dev/null +++ b/cartographer/cartographer/sensor/point_cloud_test.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/sensor/point_cloud.h" + +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +TEST(PointCloudTest, TransformPointCloud2D) { + PointCloud2D point_cloud; + point_cloud.emplace_back(0.5f, 0.5f); + point_cloud.emplace_back(3.5f, 0.5f); + const PointCloud2D transformed_point_cloud = + TransformPointCloud2D(point_cloud, transform::Rigid2f::Rotation(M_PI_2)); + EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6); + EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6); + EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/proto/CMakeLists.txt b/cartographer/cartographer/sensor/proto/CMakeLists.txt new file mode 100644 index 0000000..f32e838 --- /dev/null +++ b/cartographer/cartographer/sensor/proto/CMakeLists.txt @@ -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. + +google_proto_library(sensor_proto_adaptive_voxel_filter_options + SRCS + adaptive_voxel_filter_options.proto +) + +google_proto_library(sensor_proto_configuration + SRCS + configuration.proto + DEPENDS + transform_proto_transform +) + +google_proto_library(sensor_proto_sensor + SRCS + sensor.proto + DEPENDS + transform_proto_transform +) diff --git a/cartographer/cartographer/sensor/proto/adaptive_voxel_filter_options.proto b/cartographer/cartographer/sensor/proto/adaptive_voxel_filter_options.proto new file mode 100644 index 0000000..a6dcaea --- /dev/null +++ b/cartographer/cartographer/sensor/proto/adaptive_voxel_filter_options.proto @@ -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. + +syntax = "proto2"; + +package cartographer.sensor.proto; + +message AdaptiveVoxelFilterOptions { + // 'max_length' of a voxel edge. + optional float max_length = 1; + + // If there are more points and not at least 'min_num_points' remain, the + // voxel length is reduced trying to get this minimum number of points. + optional float min_num_points = 2; + + // Points further away from the origin are removed. + optional float max_range = 3; +}; diff --git a/cartographer/cartographer/sensor/proto/configuration.proto b/cartographer/cartographer/sensor/proto/configuration.proto new file mode 100644 index 0000000..3b5abbe --- /dev/null +++ b/cartographer/cartographer/sensor/proto/configuration.proto @@ -0,0 +1,33 @@ +// 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.sensor.proto; + +import "cartographer/transform/proto/transform.proto"; + +// NEXT_ID: 16 +message Configuration { + // NEXT_ID: 4 + message Sensor { + // 'frame_id' of the sensor. + optional string frame_id = 2; + + // Transformation from 'frame_id' to the tracking frame. + optional transform.proto.Rigid3d transform = 3; + } + + repeated Sensor sensor = 15; +} diff --git a/cartographer/cartographer/sensor/proto/sensor.proto b/cartographer/cartographer/sensor/proto/sensor.proto new file mode 100644 index 0000000..614fb7b --- /dev/null +++ b/cartographer/cartographer/sensor/proto/sensor.proto @@ -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. + +syntax = "proto2"; + +package cartographer.sensor.proto; + +option java_outer_classname = "Sensor"; + +import "cartographer/transform/proto/transform.proto"; + +// Collection of 3D 'points'. +message PointCloud { + // Points as repeated floats for efficiency. All fields have the same size. + repeated float x = 3 [packed = true]; + repeated float y = 4 [packed = true]; + repeated float z = 5 [packed = true]; +} + +// Compressed variant of PointCloud. +message CompressedPointCloud { + optional int32 num_points = 1; + repeated int32 point_data = 3 [packed = true]; +} + +// Linearized matrix representation in row major format. +// E.g. the 2x3 matrix: +// | 1, 2, 3 | +// | 4, 5, 6 | +// would be represented as +// { +// rows: 2; +// cols: 2; +// data: [1, 2, 3, 4, 5, 6] +// } +message Matrix { + optional int32 rows = 1; + optional int32 cols = 2; + repeated double data = 3; +} + +// Modeled after ROS's MultiEchoLaserScan message. +// http://docs.ros.org/api/sensor_msgs/html/msg/MultiEchoLaserScan.html +message LaserScan { + message Values { + repeated float value = 1 [packed = true]; + } + + optional float angle_min = 2; + optional float angle_max = 3; + optional float angle_increment = 4; + optional float time_increment = 5; + optional float scan_time = 6; + optional float range_min = 7; + optional float range_max = 8; + + repeated Values range = 9; + repeated Values intensity = 10; +} + +// Proto representation of ::cartographer::sensor::LaserFan3D +message LaserFan3D { + optional transform.proto.Vector3f origin = 1; + optional PointCloud point_cloud = 2; + optional PointCloud missing_echo_point_cloud = 3; + + // Reflectivity values of point_cloud or empty. + repeated int32 reflectivity = 4 [packed = true]; +} + +// IMU measurement. +message Imu { + optional transform.proto.Quaterniond orientation = 2; + optional transform.proto.Vector3d angular_velocity = 3; + optional transform.proto.Vector3d linear_acceleration = 4; + optional transform.proto.Vector3d magnetic_field = 5; + optional float ambient_pressure = 6; + // Rotation in radians in the sensor frame since the previous measurement. + optional transform.proto.Vector3d delta_rotation = 7; + // Change in velocity in the sensor frame since the previous measurement. + optional transform.proto.Vector3d delta_velocity = 8; +} diff --git a/cartographer/cartographer/sensor/sensor_packet_period_histogram_builder.cc b/cartographer/cartographer/sensor/sensor_packet_period_histogram_builder.cc new file mode 100644 index 0000000..38f9979 --- /dev/null +++ b/cartographer/cartographer/sensor/sensor_packet_period_histogram_builder.cc @@ -0,0 +1,84 @@ +/* + * 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/sensor/sensor_packet_period_histogram_builder.h" + +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { +namespace { + +string ToBucket(int ticks) { + if (ticks < 1 * 10000) { + return "< 1ms"; + } else if (ticks < 3 * 10000) { + return "< 3ms"; + } else if (ticks < 5 * 10000) { + return "< 5ms"; + } else if (ticks < 7 * 10000) { + return "< 7ms"; + } else if (ticks < 10 * 10000) { + return "< 10ms"; + } else if (ticks < 30 * 10000) { + return "< 30ms"; + } else if (ticks < 100 * 10000) { + return "< 100ms"; + } else if (ticks < 500 * 10000) { + return "< 500ms"; + } + return "> 500ms"; +} + +} // namespace + +void SensorPacketPeriodHistogramBuilder::Add(const int trajectory_id, + const int64 timestamp, + const string& frame_id) { + if (histograms_.count(trajectory_id) == 0) { + histograms_.emplace(trajectory_id, + std::unordered_map()); + } + if (histograms_.at(trajectory_id).count(frame_id) == 0) { + histograms_.at(trajectory_id).emplace(frame_id, common::BucketHistogram()); + } + const Key key = std::make_pair(trajectory_id, frame_id); + if (last_timestamps_.count(key) != 0) { + const int64 previous_timestamp = last_timestamps_.at(key); + histograms_.at(trajectory_id) + .at(frame_id) + .Hit(ToBucket(timestamp - previous_timestamp)); + } + last_timestamps_[key] = timestamp; +} + +void SensorPacketPeriodHistogramBuilder::LogHistogramsAndClear() { + for (const auto& trajectory_map_entry : histograms_) { + LOG(INFO) << "Printing histograms for trajectory with id " + << trajectory_map_entry.first; + for (const auto& frame_id_to_histogram_map : trajectory_map_entry.second) { + LOG(INFO) << "Sensor packet period histogram for '" + << frame_id_to_histogram_map.first << "' from trajectory '" + << trajectory_map_entry.first << "':\n" + << frame_id_to_histogram_map.second.ToString(); + } + } + histograms_.clear(); + last_timestamps_.clear(); +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/sensor_packet_period_histogram_builder.h b/cartographer/cartographer/sensor/sensor_packet_period_histogram_builder.h new file mode 100644 index 0000000..b51ff1a --- /dev/null +++ b/cartographer/cartographer/sensor/sensor_packet_period_histogram_builder.h @@ -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. + */ + +#ifndef CARTOGRAPHER_SENSOR_SENSOR_PACKET_PERIOD_HISTOGRAM_BUILDER_H_ +#define CARTOGRAPHER_SENSOR_SENSOR_PACKET_PERIOD_HISTOGRAM_BUILDER_H_ + +#include +#include + +#include "cartographer/common/histogram.h" +#include "cartographer/common/port.h" + +namespace cartographer { +namespace sensor { + +class SensorPacketPeriodHistogramBuilder { + public: + void Add(int trajectory_id, int64 timestamp, const string& frame_id); + void LogHistogramsAndClear(); + + private: + using Key = std::pair; + + std::map last_timestamps_; + std::unordered_map> + histograms_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_SENSOR_PACKET_PERIOD_HISTOGRAM_BUILDER_H_ diff --git a/cartographer/cartographer/sensor/voxel_filter.cc b/cartographer/cartographer/sensor/voxel_filter.cc new file mode 100644 index 0000000..239010b --- /dev/null +++ b/cartographer/cartographer/sensor/voxel_filter.cc @@ -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. + */ + +#include "cartographer/sensor/voxel_filter.h" + +#include + +#include "cartographer/common/math.h" + +namespace cartographer { +namespace sensor { + +namespace { + +template +PointCloudType FilterByMaxRange(const PointCloudType& point_cloud, + const float max_range) { + PointCloudType result; + for (const auto& point : point_cloud) { + if (point.norm() <= max_range) { + result.push_back(point); + } + } + return result; +} + +template +PointCloudType AdaptivelyVoxelFiltered( + const proto::AdaptiveVoxelFilterOptions& options, + const PointCloudType& point_cloud) { + if (point_cloud.size() <= options.min_num_points()) { + // 'point_cloud' is already sparse enough. + return point_cloud; + } + PointCloudType result = VoxelFiltered(point_cloud, options.max_length()); + if (result.size() >= options.min_num_points()) { + // Filtering with 'max_length' resulted in a sufficiently dense point cloud. + return result; + } + // Search for a 'low_length' that is known to result in a sufficiently + // dense point cloud. We give up and use the full 'point_cloud' if reducing + // the edge length by a factor of 1e-2 is not enough. + for (float high_length = options.max_length(); + high_length > 1e-2f * options.max_length(); high_length /= 2.f) { + float low_length = high_length / 2.f; + result = VoxelFiltered(point_cloud, low_length); + if (result.size() >= options.min_num_points()) { + // Binary search to find the right amount of filtering. 'low_length' gave + // a sufficiently dense 'result', 'high_length' did not. We stop when the + // edge length is at most 10% off. + while ((high_length - low_length) / low_length > 1e-1f) { + const float mid_length = (low_length + high_length) / 2.f; + const PointCloudType candidate = VoxelFiltered(point_cloud, mid_length); + if (candidate.size() >= options.min_num_points()) { + low_length = mid_length; + result = candidate; + } else { + high_length = mid_length; + } + } + return result; + } + } + return result; +} + +} // namespace + +PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, const float size) { + VoxelFilter voxel_filter(size); + voxel_filter.InsertPointCloud(point_cloud); + return voxel_filter.point_cloud(); +} + +PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) { + VoxelFilter3D voxel_filter(size); + voxel_filter.InsertPointCloud(point_cloud); + return voxel_filter.point_cloud(); +} + +VoxelFilter::VoxelFilter(const float size) : size_(size) {} + +void VoxelFilter::InsertPointCloud(const PointCloud2D& point_cloud) { + for (const Eigen::Vector2f& point : point_cloud) { + if (voxels_ + .emplace(common::RoundToInt64(point.x() / size_), + common::RoundToInt64(point.y() / size_)) + .second) { + point_cloud_.push_back(point); + } + } +} + +const PointCloud2D& VoxelFilter::point_cloud() const { return point_cloud_; } + +VoxelFilter3D::VoxelFilter3D(const float size) + : voxels_(size, Eigen::Vector3f::Zero()) {} + +void VoxelFilter3D::InsertPointCloud(const PointCloud& point_cloud) { + for (const Eigen::Vector3f& point : point_cloud) { + auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point)); + if (*value == 0) { + point_cloud_.push_back(point); + *value = 1; + } + } +} + +const PointCloud& VoxelFilter3D::point_cloud() const { return point_cloud_; } + +proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::AdaptiveVoxelFilterOptions options; + options.set_max_length(parameter_dictionary->GetDouble("max_length")); + options.set_min_num_points( + parameter_dictionary->GetNonNegativeInt("min_num_points")); + options.set_max_range(parameter_dictionary->GetDouble("max_range")); + return options; +} + +AdaptiveVoxelFilter::AdaptiveVoxelFilter( + const proto::AdaptiveVoxelFilterOptions& options) + : options_(options) {} + +PointCloud2D AdaptiveVoxelFilter::Filter( + const PointCloud2D& point_cloud) const { + return AdaptivelyVoxelFiltered( + options_, FilterByMaxRange(point_cloud, options_.max_range())); +} + +PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const { + return AdaptivelyVoxelFiltered( + options_, FilterByMaxRange(point_cloud, options_.max_range())); +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/sensor/voxel_filter.h b/cartographer/cartographer/sensor/voxel_filter.h new file mode 100644 index 0000000..4f4da37 --- /dev/null +++ b/cartographer/cartographer/sensor/voxel_filter.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_SENSOR_VOXEL_FILTER_H_ +#define CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ + +#include +#include +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" + +namespace cartographer { +namespace sensor { + +// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length +// a voxel edge. +PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, float size); + +// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length +// a voxel edge. +PointCloud VoxelFiltered(const PointCloud& point_cloud, float size); + +// Voxel filter for point clouds. For each voxel, the assembled point cloud +// contains the first point that fell into it from any of the inserted point +// clouds. +class VoxelFilter { + public: + // 'size' is the length of a voxel edge. + explicit VoxelFilter(float size); + + VoxelFilter(const VoxelFilter&) = delete; + VoxelFilter& operator=(const VoxelFilter&) = delete; + + // Inserts a point cloud into the voxel filter. + void InsertPointCloud(const PointCloud2D& point_cloud); + + // Returns the filtered point cloud representing the occupied voxels. + const PointCloud2D& point_cloud() const; + + private: + struct IntegerPairHash { + size_t operator()(const std::pair& x) const { + const uint64 first = x.first; + const uint64 second = x.second; + return first ^ (first + 0x9e3779b9u + (second << 6) + (second >> 2)); + } + }; + + const float size_; + std::unordered_set, IntegerPairHash> voxels_; + PointCloud2D point_cloud_; +}; + +// The same as VoxelFilter but for 3D PointClouds. +class VoxelFilter3D { + public: + // 'size' is the length of a voxel edge. + explicit VoxelFilter3D(float size); + + VoxelFilter3D(const VoxelFilter3D&) = delete; + VoxelFilter3D& operator=(const VoxelFilter3D&) = delete; + + // Inserts a point cloud into the voxel filter. + void InsertPointCloud(const PointCloud& point_cloud); + + // Returns the filtered point cloud representing the occupied voxels. + const PointCloud& point_cloud() const; + + private: + mapping_3d::HybridGridBase voxels_; + PointCloud point_cloud_; +}; + +proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class AdaptiveVoxelFilter { + public: + explicit AdaptiveVoxelFilter( + const proto::AdaptiveVoxelFilterOptions& options); + + AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete; + AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete; + + PointCloud2D Filter(const PointCloud2D& point_cloud) const; + PointCloud Filter(const PointCloud& point_cloud) const; + + private: + const proto::AdaptiveVoxelFilterOptions options_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ diff --git a/cartographer/cartographer/sensor/voxel_filter_test.cc b/cartographer/cartographer/sensor/voxel_filter_test.cc new file mode 100644 index 0000000..e3e0964 --- /dev/null +++ b/cartographer/cartographer/sensor/voxel_filter_test.cc @@ -0,0 +1,47 @@ +/* + * 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/sensor/voxel_filter.h" + +#include + +#include "gmock/gmock.h" + +namespace cartographer { +namespace sensor { +namespace { + +using ::testing::ContainerEq; + +TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { + PointCloud2D point_cloud = { + {0.f, 0.f}, {0.1f, -0.1f}, {0.3f, -0.1f}, {0.f, 0.f}}; + EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f), + ContainerEq(PointCloud2D{point_cloud[0], point_cloud[2]})); +} + +TEST(VoxelFilter3DTest, ReturnsTheFirstPointInEachVoxel) { + PointCloud point_cloud = {{0.f, 0.f, 0.f}, + {0.1f, -0.1f, 0.1f}, + {0.3f, -0.1f, 0.f}, + {0.f, 0.f, 0.1f}}; + EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f), + ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); +} + +} // namespace +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/cartographer/transform/CMakeLists.txt b/cartographer/cartographer/transform/CMakeLists.txt new file mode 100644 index 0000000..22e623a --- /dev/null +++ b/cartographer/cartographer/transform/CMakeLists.txt @@ -0,0 +1,83 @@ +# 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(transform_rigid_transform + USES_CERES + USES_EIGEN + SRCS + rigid_transform.cc + HDRS + rigid_transform.h + DEPENDS + common_lua_parameter_dictionary + common_math + common_port +) + +google_library(transform_rigid_transform_test_helpers + USES_EIGEN + HDRS + rigid_transform_test_helpers.h + DEPENDS + common_port + transform_rigid_transform +) + +google_library(transform_transform + USES_EIGEN + SRCS + transform.cc + HDRS + transform.h + DEPENDS + common_math + transform_proto_transform + transform_rigid_transform +) + +google_library(transform_transform_interpolation_buffer + USES_CERES + USES_EIGEN + SRCS + transform_interpolation_buffer.cc + HDRS + transform_interpolation_buffer.h + DEPENDS + common_make_unique + common_time + proto_trajectory + transform_rigid_transform + transform_transform +) + +google_test(transform_transform_interpolation_buffer_test + USES_EIGEN + SRCS + transform_interpolation_buffer_test.cc + DEPENDS + transform_rigid_transform + transform_rigid_transform_test_helpers + transform_transform_interpolation_buffer +) + +google_test(transform_transform_test + SRCS + transform_test.cc + DEPENDS + transform_rigid_transform + transform_rigid_transform_test_helpers + transform_transform +) diff --git a/cartographer/cartographer/transform/proto/CMakeLists.txt b/cartographer/cartographer/transform/proto/CMakeLists.txt new file mode 100644 index 0000000..38606ae --- /dev/null +++ b/cartographer/cartographer/transform/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(transform_proto_transform + SRCS + transform.proto +) diff --git a/cartographer/cartographer/transform/proto/transform.proto b/cartographer/cartographer/transform/proto/transform.proto new file mode 100644 index 0000000..346e14e --- /dev/null +++ b/cartographer/cartographer/transform/proto/transform.proto @@ -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. + +syntax = "proto2"; + +package cartographer.transform.proto; + +// All coordinates are expressed in the right-handed Cartesian coordinate system +// used by Cartographer (x forward, y left, z up). Message names are chosen to +// mirror those used in the Eigen library. + +message Vector2d { + optional double x = 1; + optional double y = 2; +} + +message Vector2f { + optional float x = 1; + optional float y = 2; +} + +message Vector3d { + optional double x = 1; + optional double y = 2; + optional double z = 3; +} + +message Vector3f { + optional float x = 1; + optional float y = 2; + optional float z = 3; +} + +message Quaterniond { + optional double x = 1; + optional double y = 2; + optional double z = 3; + optional double w = 4; +} + +message Quaternionf { + optional float x = 1; + optional float y = 2; + optional float z = 3; + optional float w = 4; +} + +message Rigid2d { + optional Vector2d translation = 1; + optional double rotation = 2; +} + +message Rigid2f { + optional Vector2f translation = 1; + optional float rotation = 2; +} + +message Rigid3d { + optional Vector3d translation = 1; + optional Quaterniond rotation = 2; +} + +message Rigid3f { + optional Vector3f translation = 1; + optional Quaternionf rotation = 2; +} diff --git a/cartographer/cartographer/transform/rigid_transform.cc b/cartographer/cartographer/transform/rigid_transform.cc new file mode 100644 index 0000000..90e0138 --- /dev/null +++ b/cartographer/cartographer/transform/rigid_transform.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/transform/rigid_transform.h" + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "glog/logging.h" + +namespace cartographer { +namespace transform { + +namespace { + +// Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF +// specification http://wiki.ros.org/urdf/XML/joint. +Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch, + const double yaw) { + const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY()); + const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ()); + const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX()); + return yaw_angle * pitch_angle * roll_angle; +} + +Eigen::Vector3d TranslationFromDictionary( + common::LuaParameterDictionary* dictionary) { + const std::vector translation = dictionary->GetArrayValuesAsDoubles(); + CHECK_EQ(3, translation.size()) << "Need (x, y, z) for translation."; + return Eigen::Vector3d(translation[0], translation[1], translation[2]); +} + +} // namespace + +transform::Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary) { + const Eigen::Vector3d translation = + TranslationFromDictionary(dictionary->GetDictionary("translation").get()); + + auto rotation_dictionary = dictionary->GetDictionary("rotation"); + if (rotation_dictionary->HasKey("w")) { + const Eigen::Quaterniond rotation(rotation_dictionary->GetDouble("w"), + rotation_dictionary->GetDouble("x"), + rotation_dictionary->GetDouble("y"), + rotation_dictionary->GetDouble("z")); + CHECK_NEAR(rotation.norm(), 1., 1e-9); + return transform::Rigid3d(translation, rotation); + } else { + const std::vector rotation = + rotation_dictionary->GetArrayValuesAsDoubles(); + CHECK_EQ(3, rotation.size()) << "Need (roll, pitch, yaw) for rotation."; + return transform::Rigid3d( + translation, RollPitchYaw(rotation[0], rotation[1], rotation[2])); + } +} + +} // namespace transform +} // namespace cartographer diff --git a/cartographer/cartographer/transform/rigid_transform.h b/cartographer/cartographer/transform/rigid_transform.h new file mode 100644 index 0000000..98ec7e9 --- /dev/null +++ b/cartographer/cartographer/transform/rigid_transform.h @@ -0,0 +1,233 @@ +/* + * 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_TRANSFORM_RIGID_TRANSFORM_H_ +#define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ + +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/math.h" +#include "cartographer/common/port.h" + +namespace cartographer { +namespace transform { + +template +class Rigid2 { + public: + using Affine = Eigen::Transform; + using Vector = Eigen::Matrix; + using Rotation2D = Eigen::Rotation2D; + + Rigid2() + : translation_(Vector::Identity()), rotation_(Rotation2D::Identity()) {} + Rigid2(const Vector& translation, const Rotation2D& rotation) + : translation_(translation), rotation_(rotation) {} + Rigid2(const Vector& translation, const double rotation) + : translation_(translation), rotation_(rotation) {} + + static Rigid2 Rotation(const double rotation) { + return Rigid2(Vector::Zero(), rotation); + } + + static Rigid2 Rotation(const Rotation2D& rotation) { + return Rigid2(Vector::Zero(), rotation); + } + + static Rigid2 Translation(const Vector& vector) { + return Rigid2(vector, Rotation2D::Identity()); + } + + static Rigid2 Identity() { + return Rigid2(Vector::Zero(), Rotation2D::Identity()); + } + + template + Rigid2 cast() const { + return Rigid2(translation_.template cast(), + rotation_.template cast()); + } + + const Vector& translation() const { return translation_; } + + Rotation2D rotation() const { return rotation_; } + + double normalized_angle() const { + return common::NormalizeAngleDifference(rotation().angle()); + } + + Rigid2 inverse() const { + const Rotation2D rotation = rotation_.inverse(); + const Vector translation = -(rotation * translation_); + return Rigid2(translation, rotation); + } + + string DebugString() const { + string out; + out.append("{ t: ["); + out.append(std::to_string(translation().x())); + out.append(", "); + out.append(std::to_string(translation().y())); + out.append("], r: ["); + out.append(std::to_string(rotation().angle())); + out.append("] }"); + return out; + } + + private: + Vector translation_; + Rotation2D rotation_; +}; + +template +Rigid2 operator*(const Rigid2& lhs, + const Rigid2& rhs) { + return Rigid2( + lhs.rotation() * rhs.translation() + lhs.translation(), + lhs.rotation() * rhs.rotation()); +} + +template +typename Rigid2::Vector operator*( + const Rigid2& rigid, + const typename Rigid2::Vector& point) { + return rigid.rotation() * point + rigid.translation(); +} + +// This is needed for gmock. +template +std::ostream& operator<<(std::ostream& os, + const cartographer::transform::Rigid2& rigid) { + os << rigid.DebugString(); + return os; +} + +using Rigid2d = Rigid2; +using Rigid2f = Rigid2; + +template +class Rigid3 { + public: + using Affine = Eigen::Transform; + using Vector = Eigen::Matrix; + using Quaternion = Eigen::Quaternion; + using AngleAxis = Eigen::AngleAxis; + + Rigid3() + : translation_(Vector::Identity()), rotation_(Quaternion::Identity()) {} + // TODO(damonkohler): Remove + explicit Rigid3(const Affine& affine) + : translation_(affine.translation()), rotation_(affine.rotation()) {} + Rigid3(const Vector& translation, const Quaternion& rotation) + : translation_(translation), rotation_(rotation) {} + Rigid3(const Vector& translation, const AngleAxis& rotation) + : translation_(translation), rotation_(rotation) {} + + static Rigid3 Rotation(const AngleAxis& angle_axis) { + return Rigid3(Vector::Zero(), Quaternion(angle_axis)); + } + + static Rigid3 Rotation(const Quaternion& rotation) { + return Rigid3(Vector::Zero(), rotation); + } + + static Rigid3 Translation(const Vector& vector) { + return Rigid3(vector, Quaternion::Identity()); + } + + static Rigid3 Identity() { + return Rigid3(Vector::Zero(), Quaternion::Identity()); + } + + template + Rigid3 cast() const { + return Rigid3(translation_.template cast(), + rotation_.template cast()); + } + + const Vector& translation() const { return translation_; } + const Quaternion& rotation() const { return rotation_; } + + Rigid3 inverse() const { + const Quaternion rotation = rotation_.conjugate(); + const Vector translation = -(rotation * translation_); + return Rigid3(translation, rotation); + } + + string DebugString() const { + string out; + out.append("{ t: ["); + out.append(std::to_string(translation().x())); + out.append(", "); + out.append(std::to_string(translation().y())); + out.append(", "); + out.append(std::to_string(translation().z())); + out.append("], q: ["); + out.append(std::to_string(rotation().w())); + out.append(", "); + out.append(std::to_string(rotation().x())); + out.append(", "); + out.append(std::to_string(rotation().y())); + out.append(", "); + out.append(std::to_string(rotation().z())); + out.append("] }"); + return out; + } + + private: + Vector translation_; + Quaternion rotation_; +}; + +template +Rigid3 operator*(const Rigid3& lhs, + const Rigid3& rhs) { + return Rigid3( + lhs.rotation() * rhs.translation() + lhs.translation(), + lhs.rotation() * rhs.rotation()); +} + +template +typename Rigid3::Vector operator*( + const Rigid3& rigid, + const typename Rigid3::Vector& point) { + return rigid.rotation() * point + rigid.translation(); +} + +// This is needed for gmock. +template +std::ostream& operator<<(std::ostream& os, + const cartographer::transform::Rigid3& rigid) { + os << rigid.DebugString(); + return os; +} + +using Rigid3d = Rigid3; +using Rigid3f = Rigid3; + +// Returns an transform::Rigid3d given a 'dictionary' containing 'translation' +// (x, y, z) and 'rotation' which can either we an array of (roll, pitch, yaw) +// or a dictionary with (w, x, y, z) values as a quaternion. +Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary); + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ diff --git a/cartographer/cartographer/transform/rigid_transform_test_helpers.h b/cartographer/cartographer/transform/rigid_transform_test_helpers.h new file mode 100644 index 0000000..203e29a --- /dev/null +++ b/cartographer/cartographer/transform/rigid_transform_test_helpers.h @@ -0,0 +1,51 @@ +/* + * 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_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ +#define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/port.h" +#include "cartographer/transform/rigid_transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { + +template +Eigen::Transform ToEigen(const Rigid2& rigid2) { + return Eigen::Translation(rigid2.translation()) * rigid2.rotation(); +} + +template +Eigen::Transform ToEigen(const Rigid3& rigid3) { + return Eigen::Translation(rigid3.translation()) * rigid3.rotation(); +} + +MATCHER_P2(IsNearly, rigid, epsilon, + string(string(negation ? "isn't" : "is", " nearly ") + + rigid.DebugString())) { + return ToEigen(arg).isApprox(ToEigen(rigid), epsilon); +} + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ diff --git a/cartographer/cartographer/transform/transform.cc b/cartographer/cartographer/transform/transform.cc new file mode 100644 index 0000000..641b5a4 --- /dev/null +++ b/cartographer/cartographer/transform/transform.cc @@ -0,0 +1,114 @@ +/* + * 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/transform/transform.h" + +namespace cartographer { +namespace transform { + +Rigid2d ToRigid2(const proto::Rigid2d& pose) { + return Rigid2d({pose.translation().x(), pose.translation().y()}, + pose.rotation()); +} + +Eigen::Vector2d ToEigen(const proto::Vector2d& vector) { + return Eigen::Vector2d(vector.x(), vector.y()); +} + +Eigen::Vector3f ToEigen(const proto::Vector3f& vector) { + return Eigen::Vector3f(vector.x(), vector.y(), vector.z()); +} + +Eigen::Vector3d ToEigen(const proto::Vector3d& vector) { + return Eigen::Vector3d(vector.x(), vector.y(), vector.z()); +} + +Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion) { + return Eigen::Quaterniond(quaternion.w(), quaternion.x(), quaternion.y(), + quaternion.z()); +} + +proto::Rigid2d ToProto(const transform::Rigid2d& transform) { + proto::Rigid2d proto; + proto.mutable_translation()->set_x(transform.translation().x()); + proto.mutable_translation()->set_y(transform.translation().y()); + proto.set_rotation(transform.rotation().angle()); + return proto; +} + +proto::Rigid2f ToProto(const transform::Rigid2f& transform) { + proto::Rigid2f proto; + proto.mutable_translation()->set_x(transform.translation().x()); + proto.mutable_translation()->set_y(transform.translation().y()); + proto.set_rotation(transform.rotation().angle()); + return proto; +} + +proto::Rigid3d ToProto(const transform::Rigid3d& rigid) { + proto::Rigid3d proto; + *proto.mutable_translation() = ToProto(rigid.translation()); + *proto.mutable_rotation() = ToProto(rigid.rotation()); + return proto; +} + +transform::Rigid3d ToRigid3(const proto::Rigid3d& rigid) { + return transform::Rigid3d(ToEigen(rigid.translation()), + ToEigen(rigid.rotation())); +} + +proto::Rigid3f ToProto(const transform::Rigid3f& rigid) { + proto::Rigid3f proto; + *proto.mutable_translation() = ToProto(rigid.translation()); + *proto.mutable_rotation() = ToProto(rigid.rotation()); + return proto; +} + +proto::Vector3f ToProto(const Eigen::Vector3f& vector) { + proto::Vector3f proto; + proto.set_x(vector.x()); + proto.set_y(vector.y()); + proto.set_z(vector.z()); + return proto; +} + +proto::Vector3d ToProto(const Eigen::Vector3d& vector) { + proto::Vector3d proto; + proto.set_x(vector.x()); + proto.set_y(vector.y()); + proto.set_z(vector.z()); + return proto; +} + +proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion) { + proto::Quaternionf proto; + proto.set_w(quaternion.w()); + proto.set_x(quaternion.x()); + proto.set_y(quaternion.y()); + proto.set_z(quaternion.z()); + return proto; +} + +proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion) { + proto::Quaterniond proto; + proto.set_w(quaternion.w()); + proto.set_x(quaternion.x()); + proto.set_y(quaternion.y()); + proto.set_z(quaternion.z()); + return proto; +} + +} // namespace transform +} // namespace cartographer diff --git a/cartographer/cartographer/transform/transform.h b/cartographer/cartographer/transform/transform.h new file mode 100644 index 0000000..7b8c56e --- /dev/null +++ b/cartographer/cartographer/transform/transform.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_TRANSFORM_TRANSFORM_H_ +#define CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/transform/proto/transform.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace transform { + +// Returns the non-negative rotation angle in radians of the 3D transformation +// 'transform'. +template +FloatType GetAngle(const Rigid3& transform) { + return FloatType(2) * std::atan2(transform.rotation().vec().norm(), + std::abs(transform.rotation().w())); +} + +// Returns the yaw component in radians of the given 3D transformation +// 'transform'. Assuming three rotations around X, then Y, then Z axis resulted +// in the rotational component, the yaw is the angle of the Z rotation. +template +T GetYaw(const Rigid3& transform) { + const Eigen::Matrix direction = + transform.rotation() * Eigen::Matrix::UnitX(); + return atan2(direction.y(), direction.x()); +} + +// Returns an angle-axis vector (a vector with the length of the rotation angle +// pointing to the direction of the rotation axis) representing the same +// rotation as the given 'quaternion'. +template +Eigen::Matrix RotationQuaternionToAngleAxisVector( + Eigen::Quaternion quaternion) { + quaternion.normalize(); + // We choose the quaternion with positive 'w', i.e., the one with a smaller + // angle that represents this orientation. + if (quaternion.w() < 0.) { + // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 + quaternion.w() *= T(-1.); + quaternion.x() *= T(-1.); + quaternion.y() *= T(-1.); + quaternion.z() *= T(-1.); + } + // We convert the quaternion into a vector along the rotation axis with + // length of the rotation angle. + const T angle = T(2.) * atan2(quaternion.vec().norm(), quaternion.w()); + constexpr double kCutoffAngle = 1e-7; // We linearize below this angle. + const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / T(2.)); + return Eigen::Matrix(scale * quaternion.x(), scale * quaternion.y(), + scale * quaternion.z()); +} + +// Returns a quaternion representing the same rotation as the given 'angle_axis' +// vector. +template +Eigen::Quaternion AngleAxisVectorToRotationQuaternion( + Eigen::Matrix angle_axis) { + T scale = T(0.5); + T w = T(1.); + constexpr double kCutoffAngle = 1e-8; // We linearize below this angle. + if (angle_axis.squaredNorm() > kCutoffAngle) { + const T norm = angle_axis.norm(); + scale = sin(norm / 2.) / norm; + w = cos(norm / 2.); + } + const Eigen::Matrix quaternion_xyz = scale * angle_axis; + return Eigen::Quaternion(w, quaternion_xyz.x(), quaternion_xyz.y(), + quaternion_xyz.z()); +} + +// Projects 'transform' onto the XY plane. +template +Rigid2 Project2D(const Rigid3& transform) { + return Rigid2(transform.translation().template head<2>(), + GetYaw(transform)); +} + +// Embeds 'transform' into 3D space in the XY plane. +template +Rigid3 Embed3D(const Rigid2& transform) { + return Rigid3( + {transform.translation().x(), transform.translation().y(), T(0)}, + Eigen::AngleAxis(transform.rotation().angle(), + Eigen::Matrix::UnitZ())); +} + +// Conversions between Eigen and proto. +Rigid2d ToRigid2(const proto::Rigid2d& transform); +Eigen::Vector2d ToEigen(const proto::Vector2d& vector); +Eigen::Vector3f ToEigen(const proto::Vector3f& vector); +Eigen::Vector3d ToEigen(const proto::Vector3d& vector); +Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion); +proto::Rigid2d ToProto(const Rigid2d& transform); +proto::Rigid2f ToProto(const Rigid2f& transform); +proto::Rigid3d ToProto(const Rigid3d& rigid); +Rigid3d ToRigid3(const proto::Rigid3d& rigid); +proto::Rigid3f ToProto(const Rigid3f& rigid); +proto::Vector3f ToProto(const Eigen::Vector3f& vector); +proto::Vector3d ToProto(const Eigen::Vector3d& vector); +proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion); +proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion); + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ diff --git a/cartographer/cartographer/transform/transform_interpolation_buffer.cc b/cartographer/cartographer/transform/transform_interpolation_buffer.cc new file mode 100644 index 0000000..b82930f --- /dev/null +++ b/cartographer/cartographer/transform/transform_interpolation_buffer.cc @@ -0,0 +1,101 @@ +/* + * 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/transform/transform_interpolation_buffer.h" + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" +#include "cartographer/proto/trajectory.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace transform { + +void TransformInterpolationBuffer::Push(const common::Time time, + const transform::Rigid3d& transform) { + if (deque_.size() > 0) { + CHECK_GE(time, latest_time()) << "New transform is older than latest."; + } + deque_.push_back(TimestampedTransform{time, transform}); +} + +bool TransformInterpolationBuffer::Has(const common::Time time) const { + if (deque_.empty()) { + return false; + } + return earliest_time() <= time && time <= latest_time(); +} + +transform::Rigid3d TransformInterpolationBuffer::Lookup( + const common::Time time) const { + CHECK(Has(time)) << "Missing transform for: " << time; + auto start = + std::lower_bound(deque_.begin(), deque_.end(), time, + [](const TimestampedTransform& timestamped_transform, + const common::Time time) { + return timestamped_transform.time < time; + }); + auto end = start; + if (end->time == time) { + return end->transform; + } + --start; + if (start->time == time) { + return start->transform; + } + + const double duration = common::ToSeconds(end->time - start->time); + const double factor = common::ToSeconds(time - start->time) / duration; + const Eigen::Vector3d origin = + start->transform.translation() + + (end->transform.translation() - start->transform.translation()) * factor; + const Eigen::Quaterniond rotation = + Eigen::Quaterniond(start->transform.rotation()) + .slerp(factor, Eigen::Quaterniond(end->transform.rotation())); + return transform::Rigid3d(origin, rotation); +} + +common::Time TransformInterpolationBuffer::earliest_time() const { + CHECK(!empty()) << "Empty buffer."; + return deque_.front().time; +} + +common::Time TransformInterpolationBuffer::latest_time() const { + CHECK(!empty()) << "Empty buffer."; + return deque_.back().time; +} + +bool TransformInterpolationBuffer::empty() const { return deque_.empty(); } + +std::unique_ptr +TransformInterpolationBuffer::FromTrajectory( + const cartographer::proto::Trajectory& trajectory) { + auto interpolation_buffer = + common::make_unique(); + for (const cartographer::proto::Trajectory::Node& node : trajectory.node()) { + interpolation_buffer->Push(common::FromUniversal(node.timestamp()), + transform::ToRigid3(node.pose())); + } + return interpolation_buffer; +} + +} // namespace transform +} // namespace cartographer diff --git a/cartographer/cartographer/transform/transform_interpolation_buffer.h b/cartographer/cartographer/transform/transform_interpolation_buffer.h new file mode 100644 index 0000000..fda0b1f --- /dev/null +++ b/cartographer/cartographer/transform/transform_interpolation_buffer.h @@ -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. + */ + +#ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ +#define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/proto/trajectory.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace transform { + +// A time-ordered buffer of transforms that supports interpolated lookups. +class TransformInterpolationBuffer { + public: + static std::unique_ptr FromTrajectory( + const cartographer::proto::Trajectory& trajectory); + + // Adds a new transform to the buffer and removes the oldest transform if the + // buffer size limit is exceeded. + void Push(common::Time time, const transform::Rigid3d& transform); + + // Returns true if an interpolated transfrom can be computed at 'time'. + bool Has(common::Time time) const; + + // Returns an interpolated transform at 'time'. CHECK()s that a transform at + // 'time' is available. + transform::Rigid3d Lookup(common::Time time) const; + + // Returns the timestamp of the earliest transform in the buffer or 0 if the + // buffer is empty. + common::Time earliest_time() const; + + // Returns the timestamp of the earliest transform in the buffer or 0 if the + // buffer is empty. + common::Time latest_time() const; + + // Returns true if the buffer is empty. + bool empty() const; + + private: + struct TimestampedTransform { + common::Time time; + transform::Rigid3d transform; + }; + + std::deque deque_; +}; + +} // namespace transform +} // namespace cartographer + +#endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ diff --git a/cartographer/cartographer/transform/transform_interpolation_buffer_test.cc b/cartographer/cartographer/transform/transform_interpolation_buffer_test.cc new file mode 100644 index 0000000..d26b0b0 --- /dev/null +++ b/cartographer/cartographer/transform/transform_interpolation_buffer_test.cc @@ -0,0 +1,67 @@ +/* + * 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/transform/transform_interpolation_buffer.h" + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { +namespace { + +TEST(TransformInterpolationBufferTest, testHas) { + TransformInterpolationBuffer buffer; + EXPECT_FALSE(buffer.Has(common::FromUniversal(50))); + buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity()); + EXPECT_FALSE(buffer.Has(common::FromUniversal(25))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(50))); + EXPECT_FALSE(buffer.Has(common::FromUniversal(75))); + buffer.Push(common::FromUniversal(100), transform::Rigid3d::Identity()); + EXPECT_FALSE(buffer.Has(common::FromUniversal(25))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(50))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(75))); + EXPECT_TRUE(buffer.Has(common::FromUniversal(100))); + EXPECT_FALSE(buffer.Has(common::FromUniversal(125))); + EXPECT_EQ(common::FromUniversal(50), buffer.earliest_time()); + EXPECT_EQ(common::FromUniversal(100), buffer.latest_time()); +} + +TEST(TransformInterpolationBufferTest, testLookup) { + TransformInterpolationBuffer buffer; + buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity()); + // The rotation needs to be relatively small in order for the interpolation to + // remain a z-axis rotation. + buffer.Push(common::FromUniversal(100), + transform::Rigid3d::Translation(Eigen::Vector3d(10., 10., 10.)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(2., Eigen::Vector3d::UnitZ()))); + const common::Time time = common::FromUniversal(75); + const transform::Rigid3d interpolated = buffer.Lookup(time); + EXPECT_THAT( + interpolated, + IsNearly(transform::Rigid3d::Translation(Eigen::Vector3d(5., 5., 5.)) * + transform::Rigid3d::Rotation( + Eigen::AngleAxisd(1., Eigen::Vector3d::UnitZ())), + 1e-6)); +} + +} // namespace +} // namespace transform +} // namespace cartographer diff --git a/cartographer/cartographer/transform/transform_test.cc b/cartographer/cartographer/transform/transform_test.cc new file mode 100644 index 0000000..277d53c --- /dev/null +++ b/cartographer/cartographer/transform/transform_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/transform/transform.h" + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/rigid_transform_test_helpers.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace transform { +namespace { + +TEST(TransformTest, GetAngle) { + std::mt19937 rng(42); + std::uniform_real_distribution angle_distribution(0.f, M_PI); + std::uniform_real_distribution position_distribution(-1.f, 1.f); + + for (int i = 0; i != 100; ++i) { + const float angle = angle_distribution(rng); + const float x = position_distribution(rng); + const float y = position_distribution(rng); + const float z = position_distribution(rng); + const Eigen::Vector3f axis = Eigen::Vector3f(x, y, z).normalized(); + EXPECT_NEAR(angle, + GetAngle(Rigid3f::Rotation(AngleAxisVectorToRotationQuaternion( + Eigen::Vector3f(angle * axis)))), + 1e-6f); + } +} + +TEST(TransformTest, GetYaw) { + const auto rotation = + Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ())); + EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9); + EXPECT_NEAR(-1.2345, GetYaw(rotation.inverse()), 1e-9); +} + +TEST(TransformTest, GetYawAxisOrdering) { + const auto rotation = + Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.4321, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.6789, Eigen::Vector3d::UnitX())); + EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9); +} + +TEST(TransformTest, Embed3D) { + const Rigid2d rigid2d({1., 2.}, 0.); + const Rigid3d rigid3d( + Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) * + Rigid3d::Rotation(Eigen::Quaterniond(1., 2., 3., 4.).normalized())); + const Rigid3d expected = + rigid3d * Rigid3d::Translation(Eigen::Vector3d(1., 2., 0.)); + EXPECT_THAT(expected, IsNearly(rigid3d * Embed3D(rigid2d), 1e-9)); +} + +} // namespace +} // namespace transform +} // namespace cartographer diff --git a/cartographer/cmake/functions.cmake b/cartographer/cmake/functions.cmake new file mode 100644 index 0000000..25a4aca --- /dev/null +++ b/cartographer/cmake/functions.cmake @@ -0,0 +1,246 @@ +# 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(CMakeParseArguments) + +macro(_parse_arguments ARGS) + set(OPTIONS + USES_CERES + USES_EIGEN + USES_GLOG + USES_LUA + USES_BOOST + USES_WEBP + ) + set(ONE_VALUE_ARG ) + set(MULTI_VALUE_ARGS SRCS HDRS DEPENDS) + cmake_parse_arguments(ARG + "${OPTIONS}" "${ONE_VALUE_ARG}" "${MULTI_VALUE_ARGS}" ${ARGS}) +endmacro(_parse_arguments) + +macro(_common_compile_stuff VISIBILITY) + set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") + set_target_properties(${NAME} PROPERTIES + COMPILE_FLAGS ${TARGET_COMPILE_FLAGS}) + + if(ARG_USES_EIGEN) + target_include_directories("${NAME}" SYSTEM ${VISIBILITY} + "${EIGEN3_INCLUDE_DIR}") + target_link_libraries("${NAME}" "${EIGEN3_LIBRARIES}") + endif() + + if(ARG_USES_CERES) + target_include_directories("${NAME}" SYSTEM ${VISIBILITY} + "${CERES_INCLUDE_DIRS}") + target_link_libraries("${NAME}" "${CERES_LIBRARIES}") + endif() + + if(ARG_USES_LUA) + target_include_directories("${NAME}" SYSTEM ${VISIBILITY} + "${LUA_INCLUDE_DIR}") + target_link_libraries("${NAME}" "${LUA_LIBRARIES}") + endif() + + if(ARG_USES_BOOST) + target_include_directories("${NAME}" SYSTEM ${VISIBILITY} + "{Boost_INCLUDE_DIRS}") + target_link_libraries("${NAME}" "${Boost_LIBRARIES}") + endif() + + if(ARG_USES_WEBP) + target_link_libraries("${NAME}" webp) + endif() + + # Add the binary directory first, so that port.h is included after it has + # been generated. + target_include_directories("${NAME}" ${VISIBILITY} "${CMAKE_BINARY_DIR}") + target_include_directories("${NAME}" ${VISIBILITY} "${CMAKE_SOURCE_DIR}") + + foreach(DEPENDENCY ${ARG_DEPENDS}) + target_link_libraries(${NAME} ${DEPENDENCY}) + endforeach() + + # Figure out where to install the header. The logic goes like this: either + # the header is in the current binary directory (i.e. generated, like port.h) + # or in the current source directory - a regular header. It could also + # already be absolute (i.e. generated through a google_proto_library rule). + # In all cases we want to install the right header under the right subtree, + # e.g. foo/bar/baz.h.in will be installed from the binary directory as + # 'include/foo/bar/baz.h'. + foreach(HDR ${ARG_HDRS}) + if (EXISTS ${CMAKE_CURRENT_BINARY_DIR}/${HDR}) + set(ABS_FIL "${CMAKE_CURRENT_BINARY_DIR}/${HDR}") + file(RELATIVE_PATH REL_FIL ${CMAKE_BINARY_DIR} ${ABS_FIL}) + elseif(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/${HDR}) + set(ABS_FIL "${CMAKE_CURRENT_SOURCE_DIR}/${HDR}") + file(RELATIVE_PATH REL_FIL ${CMAKE_SOURCE_DIR} ${ABS_FIL}) + else() + set(ABS_FIL "${HDR}") + file(RELATIVE_PATH REL_FIL ${CMAKE_BINARY_DIR} ${ABS_FIL}) + endif() + get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY) + install( + FILES + ${ABS_FIL} + DESTINATION + include/${INSTALL_DIR} + ) + endforeach() +endmacro(_common_compile_stuff) + +# Create a static library out of other static libraries by running a GNU ar +# script over the dependencies. +function(google_combined_library NAME) + set(MULTI_VALUE_ARGS SRCS) + cmake_parse_arguments(ARG "" "" "${MULTI_VALUE_ARGS}" ${ARGN}) + + # Cmake requires a source file for each library, so we create a dummy file + # that is empty. Its creation depends on all libraries we want to include in + # our combined static library, i.e. it will be touched whenever any of them + # change, which means that our combined library is regenerated. + set(DUMMY_SOURCE ${CMAKE_CURRENT_BINARY_DIR}/${NAME}_dummy.cc) + add_custom_command( + OUTPUT ${DUMMY_SOURCE} + COMMAND cmake -E touch ${DUMMY_SOURCE} + DEPENDS ${ARG_SRCS} + ) + + add_custom_command( + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.cc" + "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${CMAKE_BINARY_DIR} -I + ${CMAKE_BINARY_DIR} ${REWRITTEN_PROTO} + DEPENDS ${REWRITTEN_PROTO} + COMMENT "Running C++ protocol buffer compiler on ${FIL}" + VERBATIM + ) + + # Just a dummy library, we will overwrite its output directly after again + # with its POST_BUILD step. + google_library(${NAME} + SRCS ${DUMMY_SOURCE} + DEPENDS ${ARG_SRCS} + ) + + get_property(OUTPUT_FILE TARGET ${NAME} PROPERTY LOCATION) + + # We will delete the static lib generated by the last call to + # 'google_library' and recreate it using a GNU ar script that combines the + # SRCS into the NAME. + # TODO(hrapp): this is probably not very portable, but should work fine on + # Linux. + set(AR_SCRIPT "") + set(AR_SCRIPT "CREATE ${OUTPUT_FILE}\n") + foreach(SRC ${ARG_SRCS}) + get_property(STATIC_LIBRARY_FILE TARGET ${SRC} PROPERTY LOCATION) + set(AR_SCRIPT "${AR_SCRIPT}ADDLIB ${STATIC_LIBRARY_FILE}\n") + endforeach() + set(AR_SCRIPT "${AR_SCRIPT}SAVE\nEND\n") + set(AR_SCRIPT_FILE "${CMAKE_CURRENT_BINARY_DIR}/${NAME}_ar.script") + file(WRITE ${AR_SCRIPT_FILE} ${AR_SCRIPT}) + + add_custom_command(TARGET ${NAME} POST_BUILD + COMMAND rm ${OUTPUT_FILE} + COMMAND ${CMAKE_AR} + ARGS -M < ${AR_SCRIPT_FILE} + COMMENT "Recombining static libraries into ${NAME}." + ) +endfunction() + +# Create a variable 'VAR_NAME'='FLAG'. If VAR_NAME is already set, FLAG is +# appended. +function(google_add_flag VAR_NAME FLAG) + if (${VAR_NAME}) + set(${VAR_NAME} "${${VAR_NAME}} ${FLAG}" PARENT_SCOPE) + else() + set(${VAR_NAME} "${FLAG}" PARENT_SCOPE) + endif() +endfunction() + +function(google_test NAME) + _parse_arguments("${ARGN}") + + add_executable(${NAME} + ${ARG_SRCS} ${ARG_HDRS} + ) + + _common_compile_stuff("PRIVATE") + + # Make sure that gmock always includes the correct gtest/gtest.h. + target_include_directories("${NAME}" SYSTEM PRIVATE + "${GMOCK_SRC_DIR}/gtest/include") + target_link_libraries("${NAME}" gmock_main) + + add_test(${NAME} ${NAME}) +endfunction() + +function(google_library NAME) + _parse_arguments("${ARGN}") + + add_library(${NAME} + STATIC + ${ARG_SRCS} ${ARG_HDRS} + ) + + # Update the global variable that contains all static libraries. + SET(ALL_LIBRARIES "${ALL_LIBRARIES};${NAME}" CACHE INTERNAL "ALL_LIBRARIES") + + # This is needed for header only libraries. While they do not really mean + # anything for cmake, they are useful to make dependencies explicit. + set_target_properties(${NAME} PROPERTIES LINKER_LANGUAGE CXX) + + _common_compile_stuff("PUBLIC") +endfunction() + +function(google_proto_library NAME) + _parse_arguments("${ARGN}") + + set(PROTO_SRCS) + set(PROTO_HDRS) + foreach(FIL ${ARG_SRCS}) + get_filename_component(ABS_FIL ${FIL} ABSOLUTE) + get_filename_component(FIL_WE ${FIL} NAME_WE) + get_filename_component(FIL_NAME ${FIL} NAME) + + list(APPEND PROTO_SRCS "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.cc") + list(APPEND PROTO_HDRS "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.h") + + add_custom_command( + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.cc" + "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.h" + COMMAND ${PROTOBUF_PROTOC_EXECUTABLE} + ARGS --cpp_out ${CMAKE_BINARY_DIR} -I + ${CMAKE_SOURCE_DIR} ${ABS_FIL} + DEPENDS ${ABS_FIL} + COMMENT "Running C++ protocol buffer compiler on ${FIL}" + VERBATIM + ) + endforeach() + + set_source_files_properties(${PROTO_SRCS} ${PROTO_HDRS} + PROPERTIES GENERATED TRUE) + + google_library("${NAME}" + SRCS "${PROTO_SRCS}" + HDRS "${PROTO_HDRS}" + DEPENDS "${ARG_DEPENDS}" + ) + + target_include_directories("${NAME}" SYSTEM "PUBLIC" + "${PROTOBUF_INCLUDE_DIR}") + # TODO(hrapp): This should not explicityly list pthread and use + # PROTOBUF_LIBRARIES, but that failed on first try. + target_link_libraries("${NAME}" "${PROTOBUF_LIBRARY}" pthread) +endfunction() diff --git a/cartographer/cmake/modules/FindLuaGoogle.cmake b/cartographer/cmake/modules/FindLuaGoogle.cmake new file mode 100644 index 0000000..6769d6d --- /dev/null +++ b/cartographer/cmake/modules/FindLuaGoogle.cmake @@ -0,0 +1,215 @@ +# 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. + +# TODO(hrapp): Take from the cmake master branch and simplified. As soon as we +# require a CMakeVersion that ships with this, remove again. + +#.rst: +# FindLua +# ------- +# +# +# +# Locate Lua library This module defines +# +# :: +# +# LUA_FOUND - if false, do not try to link to Lua +# LUA_LIBRARIES - both lua and lualib +# LUA_INCLUDE_DIR - where to find lua.h +# LUA_VERSION_STRING - the version of Lua found +# LUA_VERSION_MAJOR - the major version of Lua +# LUA_VERSION_MINOR - the minor version of Lua +# LUA_VERSION_PATCH - the patch version of Lua +# +# +# +# Note that the expected include convention is +# +# :: +# +# #include "lua.h" +# +# and not +# +# :: +# +# #include +# +# This is because, the lua location is not standardized and may exist in +# locations other than lua/ + +#============================================================================= +# Copyright 2007-2009 Kitware, Inc. +# Copyright 2013 Rolf Eike Beer +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + +unset(_lua_include_subdirs) +unset(_lua_library_names) +unset(_lua_append_versions) + +# this is a function only to have all the variables inside go away automatically +function(_lua_set_version_vars) + set(LUA_VERSIONS5 5.3 5.2 5.1 5.0) + + if (Lua_FIND_VERSION_EXACT) + if (Lua_FIND_VERSION_COUNT GREATER 1) + set(_lua_append_versions ${Lua_FIND_VERSION_MAJOR}.${Lua_FIND_VERSION_MINOR}) + endif () + elseif (Lua_FIND_VERSION) + # once there is a different major version supported this should become a loop + if (NOT Lua_FIND_VERSION_MAJOR GREATER 5) + if (Lua_FIND_VERSION_COUNT EQUAL 1) + set(_lua_append_versions ${LUA_VERSIONS5}) + else () + foreach (subver IN LISTS LUA_VERSIONS5) + if (NOT subver VERSION_LESS ${Lua_FIND_VERSION}) + list(APPEND _lua_append_versions ${subver}) + endif () + endforeach () + endif () + endif () + else () + # once there is a different major version supported this should become a loop + set(_lua_append_versions ${LUA_VERSIONS5}) + endif () + + list(APPEND _lua_include_subdirs "include/lua" "include") + + foreach (ver IN LISTS _lua_append_versions) + string(REGEX MATCH "^([0-9]+)\\.([0-9]+)$" _ver "${ver}") + list(APPEND _lua_include_subdirs + include/lua${CMAKE_MATCH_1}${CMAKE_MATCH_2} + include/lua${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + include/lua-${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + ) + list(APPEND _lua_library_names + lua${CMAKE_MATCH_1}${CMAKE_MATCH_2} + lua${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + lua-${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + lua.${CMAKE_MATCH_1}.${CMAKE_MATCH_2} + ) + endforeach () + + set(_lua_include_subdirs "${_lua_include_subdirs}" PARENT_SCOPE) + set(_lua_library_names "${_lua_library_names}" PARENT_SCOPE) + set(_lua_append_versions "${_lua_append_versions}" PARENT_SCOPE) +endfunction(_lua_set_version_vars) + +function(_lua_check_header_version _hdr_file) + # At least 5.[012] have different ways to express the version + # so all of them need to be tested. Lua 5.2 defines LUA_VERSION + # and LUA_RELEASE as joined by the C preprocessor, so avoid those. + file(STRINGS "${_hdr_file}" lua_version_strings + REGEX "^#define[ \t]+LUA_(RELEASE[ \t]+\"Lua [0-9]|VERSION([ \t]+\"Lua [0-9]|_[MR])).*") + + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION_MAJOR[ \t]+\"([0-9])\"[ \t]*;.*" "\\1" LUA_VERSION_MAJOR ";${lua_version_strings};") + if (LUA_VERSION_MAJOR MATCHES "^[0-9]+$") + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION_MINOR[ \t]+\"([0-9])\"[ \t]*;.*" "\\1" LUA_VERSION_MINOR ";${lua_version_strings};") + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION_RELEASE[ \t]+\"([0-9])\"[ \t]*;.*" "\\1" LUA_VERSION_PATCH ";${lua_version_strings};") + set(LUA_VERSION_STRING "${LUA_VERSION_MAJOR}.${LUA_VERSION_MINOR}.${LUA_VERSION_PATCH}") + else () + string(REGEX REPLACE ".*;#define[ \t]+LUA_RELEASE[ \t]+\"Lua ([0-9.]+)\"[ \t]*;.*" "\\1" LUA_VERSION_STRING ";${lua_version_strings};") + if (NOT LUA_VERSION_STRING MATCHES "^[0-9.]+$") + string(REGEX REPLACE ".*;#define[ \t]+LUA_VERSION[ \t]+\"Lua ([0-9.]+)\"[ \t]*;.*" "\\1" LUA_VERSION_STRING ";${lua_version_strings};") + endif () + string(REGEX REPLACE "^([0-9]+)\\.[0-9.]*$" "\\1" LUA_VERSION_MAJOR "${LUA_VERSION_STRING}") + string(REGEX REPLACE "^[0-9]+\\.([0-9]+)[0-9.]*$" "\\1" LUA_VERSION_MINOR "${LUA_VERSION_STRING}") + string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]).*" "\\1" LUA_VERSION_PATCH "${LUA_VERSION_STRING}") + endif () + foreach (ver IN LISTS _lua_append_versions) + if (ver STREQUAL "${LUA_VERSION_MAJOR}.${LUA_VERSION_MINOR}") + set(LUA_VERSION_MAJOR ${LUA_VERSION_MAJOR} PARENT_SCOPE) + set(LUA_VERSION_MINOR ${LUA_VERSION_MINOR} PARENT_SCOPE) + set(LUA_VERSION_PATCH ${LUA_VERSION_PATCH} PARENT_SCOPE) + set(LUA_VERSION_STRING ${LUA_VERSION_STRING} PARENT_SCOPE) + return() + endif () + endforeach () +endfunction(_lua_check_header_version) + +_lua_set_version_vars() + +if (LUA_INCLUDE_DIR AND EXISTS "${LUA_INCLUDE_DIR}/lua.h") + _lua_check_header_version("${LUA_INCLUDE_DIR}/lua.h") +endif () + +if (NOT LUA_VERSION_STRING) + foreach (subdir IN LISTS _lua_include_subdirs) + unset(LUA_INCLUDE_PREFIX CACHE) + find_path(LUA_INCLUDE_PREFIX ${subdir}/lua.h + HINTS + ENV LUA_DIR + PATHS + ~/Library/Frameworks + /Library/Frameworks + /sw # Fink + /opt/local # DarwinPorts + /opt/csw # Blastwave + /opt + ) + if (LUA_INCLUDE_PREFIX) + _lua_check_header_version("${LUA_INCLUDE_PREFIX}/${subdir}/lua.h") + if (LUA_VERSION_STRING) + set(LUA_INCLUDE_DIR "${LUA_INCLUDE_PREFIX}/${subdir}") + break() + endif () + endif () + endforeach () +endif () +unset(_lua_include_subdirs) +unset(_lua_append_versions) + +find_library(LUA_LIBRARY + NAMES ${_lua_library_names} lua + HINTS + ENV LUA_DIR + PATH_SUFFIXES lib + PATHS + ~/Library/Frameworks + /Library/Frameworks + /sw + /opt/local + /opt/csw + /opt +) +unset(_lua_library_names) + +if (LUA_LIBRARY) + # include the math library for Unix + if (UNIX AND NOT APPLE AND NOT BEOS) + find_library(LUA_MATH_LIBRARY m) + set(LUA_LIBRARIES "${LUA_LIBRARY};${LUA_MATH_LIBRARY}") + # For Windows and Mac, don't need to explicitly include the math library + else () + set(LUA_LIBRARIES "${LUA_LIBRARY}") + endif () +endif () + +# handle the QUIETLY and REQUIRED arguments and set LUA_FOUND to TRUE if +# all listed variables are TRUE +FIND_PACKAGE_HANDLE_STANDARD_ARGS(Lua + REQUIRED_VARS LUA_LIBRARIES LUA_INCLUDE_DIR + VERSION_VAR LUA_VERSION_STRING) + +mark_as_advanced(LUA_INCLUDE_DIR LUA_LIBRARY LUA_MATH_LIBRARY) diff --git a/cartographer/cmake/modules/FindSphinx.cmake b/cartographer/cmake/modules/FindSphinx.cmake new file mode 100644 index 0000000..f956f3d --- /dev/null +++ b/cartographer/cmake/modules/FindSphinx.cmake @@ -0,0 +1,24 @@ +# 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. + +# TODO(hrapp): Replace through the one of ceres + +find_program(SPHINX_EXECUTABLE + NAMES sphinx-build + PATHS + /usr/bin + DOC "Sphinx") + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Sphinx DEFAULT_MSG SPHINX_EXECUTABLE) diff --git a/cartographer/configuration_files/sparse_pose_graph.lua b/cartographer/configuration_files/sparse_pose_graph.lua new file mode 100644 index 0000000..835f98d --- /dev/null +++ b/cartographer/configuration_files/sparse_pose_graph.lua @@ -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. + +SPARSE_POSE_GRAPH = { + optimize_every_n_scans = 45, + also_match_to_new_submaps = true, + constraint_builder = { + sampling_ratio = 0.3, + max_constraint_distance = 15., + adaptive_voxel_filter = { + max_length = 0.9, + min_num_points = 100, + max_range = 50., + }, + min_score = 0.55, + global_localization_min_score = 0.6, + max_covariance_trace = 1e-8, + lower_covariance_eigenvalue_bound = 1e-11, + log_matches = false, + fast_correlative_scan_matcher = { + linear_search_window = 7., + angular_search_window = math.rad(30.), + branch_and_bound_depth = 7, + covariance_scale = 1e-6, + }, + ceres_scan_matcher = { + occupied_space_cost_functor_weight = 20., + previous_pose_translation_delta_cost_functor_weight = 10., + initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + covariance_scale = 1e-6, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 10, + num_threads = 1, + }, + }, + fast_correlative_scan_matcher_3d = { + branch_and_bound_depth = 8, + full_resolution_depth = 3, + rotational_histogram_size = 120, + min_rotational_score = 0.77, + linear_xy_search_window = 4., + linear_z_search_window = 1., + angular_search_window = math.rad(15.), + }, + ceres_scan_matcher_3d = { + occupied_space_cost_functor_weight_0 = 20., + previous_pose_translation_delta_cost_functor_weight = 10., + initial_pose_estimate_rotation_delta_cost_functor_weight = 1., + covariance_scale = 1e-6, + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 10, + num_threads = 1, + }, + }, + }, + optimization_problem = { + huber_scale = 5e-2, + acceleration_scale = 7., + rotation_scale = 3e2, + consecutive_scan_translation_penalty_factor = 1e1, + consecutive_scan_rotation_penalty_factor = 1e1, + log_solver_summary = false, + log_residual_histograms = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 50, + num_threads = 7, + }, + }, + max_num_final_iterations = 200, + global_sampling_ratio = 0.01, +} diff --git a/cartographer/configuration_files/trajectory_builder.lua b/cartographer/configuration_files/trajectory_builder.lua new file mode 100644 index 0000000..bddab28 --- /dev/null +++ b/cartographer/configuration_files/trajectory_builder.lua @@ -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. + +TRAJECTORY_BUILDER = { + expect_imu_data = true, + horizontal_laser_min_z = -0.8, + horizontal_laser_max_z = 2., + horizontal_laser_voxel_filter_size = 0.025, + + use_online_correlative_scan_matching = false, + adaptive_voxel_filter = { + max_length = 0.5, + min_num_points = 200, + max_range = 50., + }, + + real_time_correlative_scan_matcher = { + linear_search_window = 0.1, + angular_search_window = math.rad(20.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1e-1, + covariance_scale = 1., + }, + + ceres_scan_matcher = { + occupied_space_cost_functor_weight = 20., + previous_pose_translation_delta_cost_functor_weight = 1., + initial_pose_estimate_rotation_delta_cost_functor_weight = 1e2, + covariance_scale = 2.34e-4, + ceres_solver_options = { + use_nonmonotonic_steps = true, + max_num_iterations = 50, + num_threads = 1, + }, + }, + + motion_filter = { + max_time_seconds = 5., + max_distance_meters = 0.2, + max_angle_radians = math.rad(1.), + }, + + pose_tracker = { + orientation_model_variance = 5e-4, + position_model_variance = 0.000654766, + velocity_model_variance = 0.053926, + imu_gravity_time_constant = 10., + imu_gravity_variance = 1e-6, + num_odometry_states = 1000, + }, + + submaps = { + resolution = 0.05, + half_length = 200., + num_laser_fans = 90, + output_debug_images = false, + laser_fan_inserter = { + insert_free_space = true, + hit_probability = 0.55, + miss_probability = 0.49, + }, + }, +} diff --git a/cartographer/configuration_files/trajectory_builder_3d.lua b/cartographer/configuration_files/trajectory_builder_3d.lua new file mode 100644 index 0000000..2b5e4e8 --- /dev/null +++ b/cartographer/configuration_files/trajectory_builder_3d.lua @@ -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. + +MAX_3D_LASER_RANGE = 60. + +TRAJECTORY_BUILDER_3D = { + laser_min_range = 1., + laser_max_range = MAX_3D_LASER_RANGE, + scans_per_accumulation = 20, + laser_voxel_filter_size = 0.15, + + high_resolution_adaptive_voxel_filter = { + max_length = 2., + min_num_points = 150, + max_range = 15., + }, + + low_resolution_adaptive_voxel_filter = { + max_length = 4., + min_num_points = 200, + max_range = MAX_3D_LASER_RANGE, + }, + + ceres_scan_matcher = { + occupied_space_cost_functor_weight_0 = 5., + occupied_space_cost_functor_weight_1 = 30., + previous_pose_translation_delta_cost_functor_weight = 0.3, + initial_pose_estimate_rotation_delta_cost_functor_weight = 2e3, + covariance_scale = 2.34e-4, + only_optimize_yaw = false, + ceres_solver_options = { + use_nonmonotonic_steps = false, + max_num_iterations = 12, + num_threads = 1, + }, + }, + + motion_filter = { + max_time_seconds = 0.5, + max_distance_meters = 0.1, + max_angle_radians = 0.004, + }, + + submaps = { + high_resolution = 0.10, + high_resolution_max_range = 20., + low_resolution = 0.45, + num_laser_fans = 160, + laser_fan_inserter = { + hit_probability = 0.55, + miss_probability = 0.49, + num_free_space_voxels = 2, + }, + }, + + use = "KALMAN", -- or "OPTIMIZING". + kalman_local_trajectory_builder = { + pose_tracker = { + orientation_model_variance = 5e-3, + position_model_variance = 0.00654766, + velocity_model_variance = 0.53926, + -- This disables gravity alignment in local SLAM. + imu_gravity_time_constant = 1e9, + imu_gravity_variance = 0, + num_odometry_states = 1, + }, + + use_online_correlative_scan_matching = false, + real_time_correlative_scan_matcher = { + linear_search_window = 0.15, + angular_search_window = math.rad(1.), + translation_delta_cost_weight = 1e-1, + rotation_delta_cost_weight = 1e-1, + covariance_scale = 1., + }, + }, + + optimizing_local_trajectory_builder = { + high_resolution_grid_scale = 5., + low_resolution_grid_scale = 15., + velocity_scale = 4e1, + translation_scale = 1e2, + rotation_scale = 1e3, + odometry_translation_scale = 1e4, + odometry_rotation_scale = 1e4, + }, +} diff --git a/cartographer/doc/CMakeLists.txt b/cartographer/doc/CMakeLists.txt new file mode 100644 index 0000000..c650496 --- /dev/null +++ b/cartographer/doc/CMakeLists.txt @@ -0,0 +1,24 @@ +# 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. + +set(OUTPUT_DIR "${CMAKE_BINARY_DIR}/doc/html") + +add_custom_target(build_doc ALL + ${SPHINX_EXECUTABLE} -b html + ${CMAKE_CURRENT_SOURCE_DIR}/source + ${CMAKE_CURRENT_BINARY_DIR}/html + COMMENT "Building documentation." +) + +# TODO(hrapp): Install documentation? diff --git a/cartographer/doc/source/conf.py b/cartographer/doc/source/conf.py new file mode 100644 index 0000000..599844c --- /dev/null +++ b/cartographer/doc/source/conf.py @@ -0,0 +1,271 @@ +# -*- coding: utf-8 -*- + +# 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. + +# Cartographer documentation build configuration file, created by +# sphinx-quickstart on Fri Jul 8 10:41:33 2016. +# +# This file is execfile()d with the current directory set to its +# containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys +import os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.insert(0, os.path.abspath('.')) + +# -- General configuration ------------------------------------------------ + +# If your documentation needs a minimal Sphinx version, state it here. +#needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.todo', + 'sphinx.ext.mathjax', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'Cartographer' +copyright = u'2016 Google Inc' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '1.0.0' +# The full version, including alpha/beta/rc tags. +release = '1.0.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = [] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +#keep_warnings = False + +# -- Options for HTML output ---------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +#html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Output file base name for HTML help builder. +htmlhelp_basename = 'Cartographerdoc' + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + #'papersize': 'letterpaper', + + # The font size ('10pt', '11pt' or '12pt'). + #'pointsize': '10pt', + + # Additional stuff for the LaTeX preamble. + #'preamble': '', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + ('index', 'Cartographer.tex', u'Cartographer Documentation', + u'Wolfgang Hess, Damon Kohler, Holger Rapp, and others', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + +# -- Options for manual page output --------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + ('index', 'cartographer', u'Cartographer Documentation', + [u'Wolfgang Hess, Damon Kohler, Holger Rapp, and others'], 1) +] + +# If true, show URL addresses after external links. +#man_show_urls = False + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + ('index', 'Cartographer', u'Cartographer Documentation', + u'Wolfgang Hess, Damon Kohler, Holger Rapp, and others', 'Cartographer', + 'One line description of project.', 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +#texinfo_appendices = [] + +# If false, no module index is generated. +#texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +#texinfo_show_urls = 'footnote' + +# If true, do not generate a @detailmenu in the "Top" node's menu. +#texinfo_no_detailmenu = False diff --git a/cartographer/doc/source/index.rst b/cartographer/doc/source/index.rst new file mode 100644 index 0000000..f6610b7 --- /dev/null +++ b/cartographer/doc/source/index.rst @@ -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. + +.. Cartographer documentation master file, created by + sphinx-quickstart on Fri Jul 8 10:41:33 2016. + You can adapt this file completely to your liking, but it should at least + contain the root `toctree` directive. + +Welcome to Cartographer's documentation! +======================================== + +Contents: + +.. toctree:: + :maxdepth: 2 + + + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` diff --git a/cartographer/package.xml b/cartographer/package.xml new file mode 100644 index 0000000..6015f1f --- /dev/null +++ b/cartographer/package.xml @@ -0,0 +1,30 @@ + + + + cartographer + 1.0.0 + TODO(hrapp): Describe for catkin. + Google + Apache 2.0 + + catkin + cmake + + + cmake + + diff --git a/cartographer/utils/update_cmakelists.py b/cartographer/utils/update_cmakelists.py new file mode 100755 index 0000000..283820c --- /dev/null +++ b/cartographer/utils/update_cmakelists.py @@ -0,0 +1,207 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +# 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. + +"""A dumb CMakeLists.txt generator that relies on source name conventions.""" + +import os +from os import path +import re + + +def MakeRelative(filename, directory): + absolute_directory = path.realpath(directory) + filename = path.realpath(filename) + return path.relpath(filename, absolute_directory) + + +def MaybeUseCmakeFile(filename): + cmake_filename = filename + ".cmake" + return cmake_filename if os.path.exists(cmake_filename) else filename + + +class Target(object): + """Container for data for a cmake target.""" + + def __init__(self, target_type, name, directory, srcs, hdrs): + self.type = target_type + self.name = name + self.directory = directory + self.srcs = srcs + self.hdrs = hdrs + self.depends = set() + self.uses = set() + + def __repr__(self): + return "%s(%s, nsrcs:%s, %s)" % (self.type, self.name, len(self.srcs), + self.depends) + + def Format(self, directory): + """Formats the target for writing into a CMakeLists.txt file.""" + lines = ["%s(%s" % (self.type, self.name),] + + for use in sorted(self.uses): + lines.append(" " + use) + + if self.srcs: + lines.append(" SRCS") + lines.extend(" " + MakeRelative(s, directory) + for s in sorted(self.srcs)) + + if self.hdrs: + lines.append(" HDRS") + lines.extend(" " + MakeRelative(s, directory) + for s in sorted(self.hdrs)) + + if self.depends: + lines.append(" DEPENDS") + lines.extend(" " + s for s in sorted(self.depends)) + lines.append(")") + return "\n".join(lines) + + +def ExtractCartographerIncludes(source): + """Returns all locally included files.""" + includes = set() + for line in open(MaybeUseCmakeFile(source)): + if source.endswith(".proto"): + match = re.match(r'^import "(cartographer/[^"]+)', line) + else: + match = re.match(r'^#include "(cartographer/[^"]+)"', line) + if match: + includes.add(match.group(1)) + return includes + + +def ExtractUses(source): + """Finds the options for the third_party libraries used.""" + uses = set() + for line in open(MaybeUseCmakeFile(source)): + if re.match(r'^#include "Eigen/', line): + uses.add("USES_EIGEN") + if re.match(r"^#include ", line): + uses.add("USES_LUA") + if re.match(r'^#include "(ceres|glog)/', line): + # We abuse Ceres CFLAGS for other Google libraries that we all depend on. + # The alternative is to ship and maintain our own Find*.cmake files which + # is not appealing. + uses.add("USES_CERES") + if re.match(r"^#include + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + map_frame: "map" + tracking_frame: "base_link" + configuration_files_directories: [ + "$(find google_cartographer)/configuration_files" + ] + mapping_configuration_basename: "3d_mapping.lua" + imu_topic: "/imu" + laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"] + laser_min_range_m: 0. + laser_max_range_m: 30. + laser_missing_echo_ray_length_m: 5. + + + + diff --git a/ros/google_cartographer/launch/state.launch b/ros/google_cartographer/launch/state.launch new file mode 100644 index 0000000..c07f760 --- /dev/null +++ b/ros/google_cartographer/launch/state.launch @@ -0,0 +1,73 @@ + + + + + + + + + + # This node publishes the transformation between the tracking and map + # frames. + map_frame: "map" + tracking_frame: "base_link" + + # Lua configuration files are always searched in the cartographer + # installation directory. We also ship configuration that work well for + # the platforms that we used to collect our example data. You probably + # want to add your own configuration overwrites in your own node + # directories - you should add the path here as first entry then. + configuration_files_directories: [ + "$(find google_cartographer)/configuration_files" + ] + + # Configuration file for SLAM. The settings in here are tweaked to the + # collection platform you are using. + mapping_configuration_basename: "backpack_3d.lua" + + imu_topic: "/imu" + + # Laser min and max range. Everything not in this range will not be used for mapping. + laser_min_range_m: 0. + laser_max_range_m: 30. + + # Missing laser echoes will be inserted as free space at this distance. + laser_missing_echo_ray_length_m: 5. + + # Only choose one in the next parameter block + # laser_scan_2d_topic: "/horizontal_2d_laser" + # multi_echo_laser_scan_2d_topic: "/horizontal_multiecho_2d_laser" + laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"] + + + + + diff --git a/ros/google_cartographer/launch/turtlebot.launch b/ros/google_cartographer/launch/turtlebot.launch new file mode 100644 index 0000000..75c9576 --- /dev/null +++ b/ros/google_cartographer/launch/turtlebot.launch @@ -0,0 +1,41 @@ + + + + + + + + + + map_frame: "map" + tracking_frame: "base_link" + configuration_files_directories: [ + "$(find google_cartographer)/configuration_files" + ] + mapping_configuration_basename: "turtlebot.lua" + imu_topic: "/imu" + laser_scan_2d_topic: "/horizontal_2d_laser" + laser_min_range_m: 0. + laser_max_range_m: 30. + laser_missing_echo_ray_length_m: 5. + + + + diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program b/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program new file mode 100644 index 0000000..045f381 --- /dev/null +++ b/ros/google_cartographer/ogre_media/materials/glsl120/glsl120.program @@ -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. + */ + +fragment_program google_cartographer/glsl120/submap.frag glsl +{ + source submap.frag +} + +vertex_program google_cartographer/glsl120/submap.vert glsl +{ + source submap.vert +} + +fragment_program google_cartographer/glsl120/screen_blit.frag glsl +{ + source screen_blit.frag +} + +vertex_program google_cartographer/glsl120/screen_blit.vert glsl +{ + source screen_blit.vert +} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag new file mode 100644 index 0000000..195d813 --- /dev/null +++ b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.frag @@ -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. + */ + +#version 120 + +varying vec2 out_texture_coordinate; + +uniform sampler2D u_texture; + +void main() +{ + vec2 texture_color = texture2D(u_texture, out_texture_coordinate).rg; + float opacity = texture_color.g; + float value = texture_color.r; + gl_FragColor = vec4(value, value, value, opacity); +} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert new file mode 100644 index 0000000..74d1bba --- /dev/null +++ b/ros/google_cartographer/ogre_media/materials/glsl120/screen_blit.vert @@ -0,0 +1,28 @@ +/* + * 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. + */ + +#version 120 + +attribute vec4 vertex; +attribute vec4 uv0; + +varying vec2 out_texture_coordinate; + +void main() +{ + out_texture_coordinate = vec2(uv0); + gl_Position = vertex; +} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag b/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag new file mode 100644 index 0000000..1998f53 --- /dev/null +++ b/ros/google_cartographer/ogre_media/materials/glsl120/submap.frag @@ -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. + */ + +#version 120 + +varying vec2 out_submap_texture_coordinate; + +uniform sampler2D u_submap; +uniform float u_alpha; + +void main() +{ + vec2 texture_value = texture2D(u_submap, out_submap_texture_coordinate).rg; + float value = texture_value.r; + float alpha = texture_value.g; + float is_known = step(1e-3, alpha + value); + gl_FragColor = vec4(u_alpha * value, u_alpha * is_known, 0., u_alpha * alpha); +} diff --git a/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert b/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert new file mode 100644 index 0000000..864bfa1 --- /dev/null +++ b/ros/google_cartographer/ogre_media/materials/glsl120/submap.vert @@ -0,0 +1,27 @@ +/* + * 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. + */ + +#version 120 + +attribute vec4 uv0; + +varying vec2 out_submap_texture_coordinate; + +void main() +{ + out_submap_texture_coordinate = vec2(uv0); + gl_Position = ftransform(); +} diff --git a/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material b/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material new file mode 100644 index 0000000..cd850fd --- /dev/null +++ b/ros/google_cartographer/ogre_media/materials/scripts/screen_blit.material @@ -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. + */ + +material google_cartographer/ScreenBlit +{ + technique + { + pass + { + vertex_program_ref google_cartographer/glsl120/screen_blit.vert {} + + fragment_program_ref google_cartographer/glsl120/screen_blit.frag + { + param_named u_texture int 0 + } + } + } +} diff --git a/ros/google_cartographer/ogre_media/materials/scripts/submap.material b/ros/google_cartographer/ogre_media/materials/scripts/submap.material new file mode 100644 index 0000000..1304156 --- /dev/null +++ b/ros/google_cartographer/ogre_media/materials/scripts/submap.material @@ -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. + */ + +material google_cartographer/Submap +{ + technique + { + pass + { + vertex_program_ref google_cartographer/glsl120/submap.vert {} + + fragment_program_ref google_cartographer/glsl120/submap.frag + { + param_named u_submap int 0 + param_named u_alpha float 1.0 + } + } + } +} diff --git a/ros/google_cartographer/package.xml b/ros/google_cartographer/package.xml new file mode 100644 index 0000000..bf3d0b9 --- /dev/null +++ b/ros/google_cartographer/package.xml @@ -0,0 +1,68 @@ + + + + + google_cartographer + 1.0.0 + The google_cartographer package. + Google + Apache 2.0 + + catkin + + eigen_conversions + geometry_msgs + google_cartographer_msgs + libpcl-all-dev + message_generation + nav_msgs + pcl_conversions + qtbase5-dev + rosbag + roscpp + roslib + rviz + sensor_msgs + std_msgs + tf2 + tf2_eigen + visualization_msgs + + eigen_conversions + geometry_msgs + google_cartographer_msgs + libpcl-all + libqt5-core + libqt5-gui + libqt5-widgets + message_runtime + nav_msgs + pcl_conversions + rosbag + roscpp + roslib + rviz + sensor_msgs + std_msgs + tf2_eigen + tf2 + visualization_msgs + + + + + diff --git a/ros/google_cartographer/rviz_plugin_description.xml b/ros/google_cartographer/rviz_plugin_description.xml new file mode 100644 index 0000000..b990bd7 --- /dev/null +++ b/ros/google_cartographer/rviz_plugin_description.xml @@ -0,0 +1,25 @@ + + + + + + Displays Google Cartographer's submaps as a unified map in RViz. + + + diff --git a/ros/google_cartographer/src/cartographer_node_main.cc b/ros/google_cartographer/src/cartographer_node_main.cc new file mode 100644 index 0000000..5dc619a --- /dev/null +++ b/ros/google_cartographer/src/cartographer_node_main.cc @@ -0,0 +1,644 @@ +/* + * 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 +#include +#include +#include + +#include "Eigen/Core" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/mutex.h" +#include "cartographer/common/port.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping/proto/submaps.pb.h" +#include "cartographer/mapping/sensor_collator.h" +#include "cartographer/mapping_2d/global_trajectory_builder.h" +#include "cartographer/mapping_2d/local_trajectory_builder.h" +#include "cartographer/mapping_2d/sparse_pose_graph.h" +#include "cartographer/mapping_3d/global_trajectory_builder.h" +#include "cartographer/mapping_3d/local_trajectory_builder.h" +#include "cartographer/mapping_3d/local_trajectory_builder_options.h" +#include "cartographer/mapping_3d/sparse_pose_graph.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/TransformStamped.h" +#include "glog/log_severity.h" +#include "glog/logging.h" +#include "google_cartographer_msgs/SubmapEntry.h" +#include "google_cartographer_msgs/SubmapList.h" +#include "google_cartographer_msgs/SubmapQuery.h" +#include "google_cartographer_msgs/TrajectorySubmapList.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" +#include "ros/ros.h" +#include "ros/serialization.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/PointCloud2.h" +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" + +#include "msg_conversion.h" +#include "node_constants.h" +#include "time_conversion.h" + +namespace cartographer_ros { +namespace { + +using ::cartographer::transform::Rigid3d; +namespace proto = ::cartographer::sensor::proto; + +// TODO(hrapp): Support multi trajectory mapping. +constexpr int64 kTrajectoryId = 0; +constexpr int kSubscriberQueueSize = 150; +constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds +constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds + +Rigid3d ToRidig3d(const geometry_msgs::TransformStamped& transform) { + return Rigid3d(Eigen::Vector3d(transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z), + Eigen::Quaterniond(transform.transform.rotation.w, + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z)); +} + +// TODO(hrapp): move to msg_conversion.cc. +geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid) { + geometry_msgs::Transform transform; + transform.translation.x = rigid.translation().x(); + transform.translation.y = rigid.translation().y(); + transform.translation.z = rigid.translation().z(); + transform.rotation.w = rigid.rotation().w(); + transform.rotation.x = rigid.rotation().x(); + transform.rotation.y = rigid.rotation().y(); + transform.rotation.z = rigid.rotation().z(); + return transform; +} + +geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid) { + geometry_msgs::Pose pose; + pose.position.x = rigid.translation().x(); + pose.position.y = rigid.translation().y(); + pose.position.z = rigid.translation().z(); + pose.orientation.w = rigid.rotation().w(); + pose.orientation.x = rigid.rotation().x(); + pose.orientation.y = rigid.rotation().y(); + pose.orientation.z = rigid.rotation().z(); + return pose; +} + +// This type is a logical union, i.e. only one proto is actually filled in. It +// is only used for time ordering sensor data before passing it on. +enum class SensorType { kImu, kLaserScan, kLaserFan3D }; +struct SensorData { + SensorData(const string& init_frame_id, proto::Imu init_imu) + : type(SensorType::kImu), frame_id(init_frame_id), imu(init_imu) {} + SensorData(const string& init_frame_id, proto::LaserScan init_laser_scan) + : type(SensorType::kLaserScan), + frame_id(init_frame_id), + laser_scan(init_laser_scan) {} + SensorData(const string& init_frame_id, proto::LaserFan3D init_laser_fan_3d) + : type(SensorType::kLaserFan3D), + frame_id(init_frame_id), + laser_fan_3d(init_laser_fan_3d) {} + + SensorType type; + string frame_id; + proto::Imu imu; + proto::LaserScan laser_scan; + proto::LaserFan3D laser_fan_3d; +}; + +// Node that listens to all the sensor data that we are interested in and wires +// it up to the SLAM. +class Node { + public: + Node(); + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + + void SpinForever(); + void Initialize(); + + private: + void HandleSensorData(int64 timestamp, + std::unique_ptr sensor_data); + void ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg); + void LaserScanMessageCallback(const sensor_msgs::LaserScan::ConstPtr& msg); + void MultiEchoLaserScanCallback( + const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); + void PointCloud2MessageCallback( + const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg); + void AddImu(int64 timestamp, const string& frame_id, const proto::Imu& imu); + void AddHorizontalLaserFan(int64 timestamp, const string& frame_id, + const proto::LaserScan& laser_scan); + void AddLaserFan3D(int64 timestamp, const string& frame_id, + const proto::LaserFan3D& laser_fan_3d); + + template + const T GetParamOrDie(const string& name); + + // Returns true if a transform for 'frame_id' to 'tracking_frame_' exists at + // 'time'. + bool CanTransform(ros::Time time, const string& frame_id); + + Rigid3d LookupToTrackingTransformOrDie(ros::Time time, + const string& frame_id); + + bool HandleSubmapQuery( + ::google_cartographer_msgs::SubmapQuery::Request& request, + ::google_cartographer_msgs::SubmapQuery::Response& response); + + void PublishSubmapList(int64 timestamp); + void PublishPose(int64 timestamp); + + // TODO(hrapp): Pull out the common functionality between this and MapWriter + // into an open sourcable MapWriter. + ::cartographer::mapping::SensorCollator sensor_collator_; + ros::NodeHandle node_handle_; + ros::Subscriber imu_subscriber_; + ros::Subscriber laser_2d_subscriber_; + std::vector laser_3d_subscribers_; + string tracking_frame_; + string map_frame_; + double laser_min_range_m_; + double laser_max_range_m_; + double laser_missing_echo_ray_length_m_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + ::cartographer::common::ThreadPool thread_pool_; + int64 last_pose_publish_timestamp_; + ::cartographer::common::Mutex mutex_; + std::unique_ptr<::cartographer::mapping::GlobalTrajectoryBuilderInterface> + trajectory_builder_ GUARDED_BY(mutex_); + std::deque<::cartographer::mapping::TrajectoryNode::ConstantData> + constant_node_data_ GUARDED_BY(mutex_); + std::unique_ptr<::cartographer::mapping::SparsePoseGraph> sparse_pose_graph_; + + ::ros::Publisher submap_list_publisher_; + int64 last_submap_list_publish_timestamp_; + ::ros::ServiceServer submap_query_server_; +}; + +Node::Node() + : node_handle_("~"), + tf_buffer_(ros::Duration(1000)), + tf_(tf_buffer_), + thread_pool_(10), + last_submap_list_publish_timestamp_(0), + last_pose_publish_timestamp_(0) {} + +bool Node::CanTransform(ros::Time time, const string& frame_id) { + return tf_buffer_.canTransform(tracking_frame_, frame_id, time); +} + +Rigid3d Node::LookupToTrackingTransformOrDie(ros::Time time, + const string& frame_id) { + geometry_msgs::TransformStamped stamped_transform; + try { + stamped_transform = tf_buffer_.lookupTransform(tracking_frame_, frame_id, + time, ros::Duration(1.)); + } catch (tf2::TransformException& ex) { + LOG(FATAL) << "Timed out while waiting for transform: " << frame_id + << " -> " << tracking_frame_ << ": " << ex.what(); + } + return ToRidig3d(stamped_transform); +} + +void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) { + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToCartographer(*msg)); + sensor_collator_.AddSensorData( + kTrajectoryId, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), + imu_subscriber_.getTopic(), std::move(sensor_data)); +} + +void Node::AddImu(const int64 timestamp, const string& frame_id, + const proto::Imu& imu) { + const ::cartographer::common::Time time = + ::cartographer::common::FromUniversal(timestamp); + + if (!CanTransform(ToRos(time), frame_id)) { + LOG(WARNING) << "Cannot transform to " << frame_id; + return; + } + const Rigid3d sensor_to_tracking = + LookupToTrackingTransformOrDie(ToRos(time), frame_id); + CHECK(sensor_to_tracking.translation().norm() < 1e-5) + << "The IMU is not colocated with the tracking frame. This makes it hard " + "and inprecise to transform its linear accelaration into the " + "tracking_frame and will decrease the quality of the SLAM."; + trajectory_builder_->AddImuData( + time, sensor_to_tracking.rotation() * + ::cartographer::transform::ToEigen(imu.linear_acceleration()), + sensor_to_tracking.rotation() * + ::cartographer::transform::ToEigen(imu.angular_velocity())); +} + +void Node::LaserScanMessageCallback( + const sensor_msgs::LaserScan::ConstPtr& msg) { + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToCartographer(*msg)); + sensor_collator_.AddSensorData( + kTrajectoryId, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), + laser_2d_subscriber_.getTopic(), std::move(sensor_data)); +} + +void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, + const proto::LaserScan& laser_scan) { + const ::cartographer::common::Time time = + ::cartographer::common::FromUniversal(timestamp); + if (!CanTransform(ToRos(time), frame_id)) { + LOG(WARNING) << "Cannot transform to " << frame_id; + return; + } + const Rigid3d sensor_to_tracking = + LookupToTrackingTransformOrDie(ToRos(time), frame_id); + + // TODO(hrapp): Make things configurable? Through Lua? Through ROS params? + const ::cartographer::sensor::LaserFan laser_fan = + ::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_, + laser_max_range_m_, + laser_missing_echo_ray_length_m_); + + const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D( + ::cartographer::sensor::ToLaserFan3D(laser_fan), + sensor_to_tracking.cast()); + trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d); +} + +void Node::MultiEchoLaserScanCallback( + const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + // TODO(hrapp): Do something useful. + LOG(INFO) << "LaserScan message: " << msg->header.stamp; +} + +void Node::PointCloud2MessageCallback( + const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) { + pcl::PointCloud pcl_points; + pcl::fromROSMsg(*msg, pcl_points); + + auto sensor_data = ::cartographer::common::make_unique( + msg->header.frame_id, ToCartographer(pcl_points)); + sensor_collator_.AddSensorData( + kTrajectoryId, + ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, + std::move(sensor_data)); +} + +void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id, + const proto::LaserFan3D& laser_fan_3d) { + const ::cartographer::common::Time time = + ::cartographer::common::FromUniversal(timestamp); + if (!CanTransform(ToRos(time), frame_id)) { + LOG(WARNING) << "Cannot transform to " << frame_id; + return; + } + const Rigid3d sensor_to_tracking = + LookupToTrackingTransformOrDie(ToRos(time), frame_id); + + trajectory_builder_->AddLaserFan3D( + time, ::cartographer::sensor::TransformLaserFan3D( + ::cartographer::sensor::FromProto(laser_fan_3d), + sensor_to_tracking.cast())); +} + +template +const T Node::GetParamOrDie(const string& name) { + CHECK(node_handle_.hasParam(name)) << "Required parameter '" << name + << "' is unset."; + T value; + node_handle_.getParam(name, value); + return value; +} + +void Node::Initialize() { + tracking_frame_ = GetParamOrDie("tracking_frame"); + map_frame_ = GetParamOrDie("map_frame"); + laser_min_range_m_ = GetParamOrDie("laser_min_range_m"); + laser_max_range_m_ = GetParamOrDie("laser_max_range_m"); + laser_missing_echo_ray_length_m_ = + GetParamOrDie("laser_missing_echo_ray_length_m"); + + // Subscribe to the IMU. + const string imu_topic = GetParamOrDie("imu_topic"); + imu_subscriber_ = node_handle_.subscribe(imu_topic, kSubscriberQueueSize, + &Node::ImuMessageCallback, this); + std::unordered_set expected_sensor_identifiers; + expected_sensor_identifiers.insert(imu_topic); + + // Subscribe to exactly one laser. + const bool has_laser_scan_2d = node_handle_.hasParam("laser_scan_2d_topic"); + const bool has_multi_echo_laser_scan_2d = + node_handle_.hasParam("multi_echo_laser_scan_2d_topic"); + const bool has_laser_scan_3d = node_handle_.hasParam("laser_scan_3d_topics"); + + CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d + has_laser_scan_3d == + 1) + << "Parameters 'laser_scan_2d_topic', 'multi_echo_laser_scan_2d_topic' " + "and 'laser_scan_3d_topics' are mutually exclusive, but one is " + "required."; + + if (has_laser_scan_2d) { + const string topic = GetParamOrDie("laser_scan_2d_topic"); + laser_2d_subscriber_ = node_handle_.subscribe( + topic, kSubscriberQueueSize, &Node::LaserScanMessageCallback, this); + expected_sensor_identifiers.insert(topic); + } + if (has_multi_echo_laser_scan_2d) { + const string topic = + GetParamOrDie("multi_echo_laser_scan_2d_topic"); + laser_2d_subscriber_ = node_handle_.subscribe( + topic, kSubscriberQueueSize, &Node::MultiEchoLaserScanCallback, this); + expected_sensor_identifiers.insert(topic); + } + + auto file_resolver = ::cartographer::common::make_unique< + ::cartographer::common::ConfigurationFileResolver>( + GetParamOrDie>("configuration_files_directories")); + const string code = file_resolver->GetFileContentOrDie( + GetParamOrDie("mapping_configuration_basename")); + + ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver), nullptr); + if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) { + auto sparse_pose_graph_2d = ::cartographer::common::make_unique< + ::cartographer::mapping_2d::SparsePoseGraph>( + ::cartographer::mapping::CreateSparsePoseGraphOptions( + lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()), + &thread_pool_, &constant_node_data_); + trajectory_builder_ = ::cartographer::common::make_unique< + ::cartographer::mapping_2d::GlobalTrajectoryBuilder>( + ::cartographer::mapping_2d::CreateLocalTrajectoryBuilderOptions( + lua_parameter_dictionary.GetDictionary("trajectory_builder").get()), + sparse_pose_graph_2d.get()); + sparse_pose_graph_ = std::move(sparse_pose_graph_2d); + } + + if (has_laser_scan_3d) { + const auto topics = + GetParamOrDie>("laser_scan_3d_topics"); + for (const auto& topic : topics) { + laser_3d_subscribers_.push_back(node_handle_.subscribe( + topic, kSubscriberQueueSize, + boost::function( + [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { + PointCloud2MessageCallback(topic, msg); + }))); + expected_sensor_identifiers.insert(topic); + } + auto sparse_pose_graph_3d = ::cartographer::common::make_unique< + ::cartographer::mapping_3d::SparsePoseGraph>( + ::cartographer::mapping::CreateSparsePoseGraphOptions( + lua_parameter_dictionary.GetDictionary("sparse_pose_graph").get()), + &thread_pool_, &constant_node_data_); + trajectory_builder_ = ::cartographer::common::make_unique< + ::cartographer::mapping_3d::GlobalTrajectoryBuilder>( + ::cartographer::mapping_3d::CreateLocalTrajectoryBuilderOptions( + lua_parameter_dictionary.GetDictionary("trajectory_builder").get()), + sparse_pose_graph_3d.get()); + sparse_pose_graph_ = std::move(sparse_pose_graph_3d); + } + CHECK(sparse_pose_graph_ != nullptr); + + // TODO(hrapp): Add odometry subscribers here. + + sensor_collator_.AddTrajectory( + kTrajectoryId, expected_sensor_identifiers, + [this](const int64 timestamp, std::unique_ptr sensor_data) { + HandleSensorData(timestamp, std::move(sensor_data)); + }); + + submap_list_publisher_ = + node_handle_.advertise<::google_cartographer_msgs::SubmapList>( + kSubmapListTopic, 10); + submap_query_server_ = node_handle_.advertiseService( + kSubmapQueryServiceName, &Node::HandleSubmapQuery, this); +} + +bool Node::HandleSubmapQuery( + ::google_cartographer_msgs::SubmapQuery::Request& request, + ::google_cartographer_msgs::SubmapQuery::Response& response) { + if (request.trajectory_id != 0) { + return false; + } + + ::cartographer::common::MutexLocker lock(&mutex_); + // TODO(hrapp): return error messages and extract common code from MapBuilder. + ::cartographer::mapping::Submaps* submaps = trajectory_builder_->submaps(); + if (request.submap_id < 0 || request.submap_id >= submaps->size()) { + return false; + } + + ::cartographer::mapping::proto::SubmapQuery::Response response_proto; + response_proto.set_submap_id(request.submap_id); + response_proto.set_submap_version( + submaps->Get(request.submap_id)->end_laser_fan_index); + const std::vector<::cartographer::transform::Rigid3d> submap_transforms = + sparse_pose_graph_->GetSubmapTransforms(*submaps); + + submaps->SubmapToProto(request.submap_id, + sparse_pose_graph_->GetTrajectoryNodes(), + submap_transforms[request.submap_id], &response_proto); + + response.submap_id = response_proto.submap_id(); + response.submap_version = response_proto.submap_version(); + response.cells.insert(response.cells.begin(), response_proto.cells().begin(), + response_proto.cells().end()); + response.width = response_proto.width(); + response.height = response_proto.height(); + response.resolution = response_proto.resolution(); + + auto pose = ::cartographer::transform::ToRigid3(response_proto.slice_pose()); + response.slice_pose.position.x = + response_proto.slice_pose().translation().x(); + response.slice_pose.position.y = + response_proto.slice_pose().translation().y(); + response.slice_pose.position.z = + response_proto.slice_pose().translation().z(); + response.slice_pose.orientation.x = + response_proto.slice_pose().rotation().x(); + response.slice_pose.orientation.y = + response_proto.slice_pose().rotation().y(); + response.slice_pose.orientation.z = + response_proto.slice_pose().rotation().z(); + response.slice_pose.orientation.w = + response_proto.slice_pose().rotation().w(); + return true; +} + +void Node::PublishSubmapList(int64 timestamp) { + ::cartographer::common::MutexLocker lock(&mutex_); + const ::cartographer::mapping::Submaps* submaps = + trajectory_builder_->submaps(); + const std::vector<::cartographer::transform::Rigid3d> submap_transforms = + sparse_pose_graph_->GetSubmapTransforms(*submaps); + CHECK_EQ(submap_transforms.size(), submaps->size()); + + ::google_cartographer_msgs::TrajectorySubmapList ros_trajectory; + for (int i = 0; i != submaps->size(); ++i) { + ::google_cartographer_msgs::SubmapEntry ros_submap; + ros_submap.submap_version = submaps->Get(i)->end_laser_fan_index; + ros_submap.pose = ToGeometryMsgPose(submap_transforms[i]); + ros_trajectory.submap.push_back(ros_submap); + } + + ::google_cartographer_msgs::SubmapList ros_submap_list; + ros_submap_list.trajectory.push_back(ros_trajectory); + submap_list_publisher_.publish(ros_submap_list); + last_submap_list_publish_timestamp_ = timestamp; +} + +void Node::PublishPose(int64 timestamp) { + ::cartographer::common::MutexLocker lock(&mutex_); + const ::cartographer::mapping::Submaps* submaps = + trajectory_builder_->submaps(); + const ::cartographer::transform::Rigid3d odometry_to_map = + sparse_pose_graph_->GetOdometryToMapTransform(*submaps); + const auto& pose_estimate = trajectory_builder_->pose_estimate(); + + const ::cartographer::transform::Rigid3d pose = + odometry_to_map * pose_estimate.pose; + const ::cartographer::common::Time time = + ::cartographer::common::FromUniversal(timestamp); + + geometry_msgs::TransformStamped stamped_transform; + stamped_transform.header.stamp = ToRos(time); + stamped_transform.header.frame_id = map_frame_; + stamped_transform.child_frame_id = tracking_frame_; + stamped_transform.transform = ToGeometryMsgTransform(pose); + tf_broadcaster_.sendTransform(stamped_transform); + last_pose_publish_timestamp_ = timestamp; +} + +void Node::HandleSensorData(const int64 timestamp, + std::unique_ptr sensor_data) { + if (last_submap_list_publish_timestamp_ + kSubmapPublishPeriodInUts < + timestamp) { + PublishSubmapList(timestamp); + } + + if (last_pose_publish_timestamp_ + kPosePublishPeriodInUts < timestamp) { + PublishPose(timestamp); + } + + switch (sensor_data->type) { + case SensorType::kImu: + AddImu(timestamp, sensor_data->frame_id, sensor_data->imu); + return; + + case SensorType::kLaserScan: + AddHorizontalLaserFan(timestamp, sensor_data->frame_id, + sensor_data->laser_scan); + return; + + case SensorType::kLaserFan3D: + AddLaserFan3D(timestamp, sensor_data->frame_id, + sensor_data->laser_fan_3d); + return; + } + LOG(FATAL); +} + +void Node::SpinForever() { ros::spin(); } + +void Run() { + Node node; + node.Initialize(); + node.SpinForever(); +} + +const char* GetBasename(const char* filepath) { + const char* base = strrchr(filepath, '/'); + return base ? (base + 1) : filepath; +} + +// Makes Google logging use ROS logging for output while an instance of this +// class exists. +class ScopedRosLogSink : public google::LogSink { + public: + ScopedRosLogSink() : will_die_(false) { AddLogSink(this); } + ~ScopedRosLogSink() override { RemoveLogSink(this); } + + void send(google::LogSeverity severity, const char* filename, + const char* base_filename, int line, const struct ::tm* tm_time, + const char* message, size_t message_len) override { + const std::string message_string = google::LogSink::ToString( + severity, GetBasename(filename), line, tm_time, message, message_len); + switch (severity) { + case google::GLOG_INFO: + ROS_INFO_STREAM(message_string); + break; + + case google::GLOG_WARNING: + ROS_WARN_STREAM(message_string); + break; + + case google::GLOG_ERROR: + ROS_ERROR_STREAM(message_string); + break; + + case google::GLOG_FATAL: + ROS_FATAL_STREAM(message_string); + will_die_ = true; + break; + } + } + + void WaitTillSent() override { + if (will_die_) { + // Arbirarily give ROS some time to actually publish our message. + std::this_thread::sleep_for( + ::cartographer::common::FromMilliseconds(1000)); + } + } + + private: + bool will_die_; +}; + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + + ros::init(argc, argv, "cartographer_node"); + ros::start(); + + ::cartographer_ros::ScopedRosLogSink ros_log_sink; + ::cartographer_ros::Run(); + ros::shutdown(); + return 0; +} diff --git a/ros/google_cartographer/src/drawable_submap.cc b/ros/google_cartographer/src/drawable_submap.cc new file mode 100644 index 0000000..5e2a035 --- /dev/null +++ b/ros/google_cartographer/src/drawable_submap.cc @@ -0,0 +1,269 @@ +/* + * 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 "drawable_submap.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace cartographer_ros { +namespace rviz { + +namespace { + +constexpr std::chrono::milliseconds kMinQueryDelayInMs(250); +constexpr char kMapFrame[] = "/map"; +constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; +constexpr char kManualObjectPrefix[] = "ManualObjectSubmap"; +constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial"; +constexpr char kSubmapSourceMaterialName[] = "google_cartographer/Submap"; + +// Distance before which the submap will be shown at full opacity, and distance +// over which the submap will then fade out. +constexpr double kFadeOutStartDistanceInMeters = 1.; +constexpr double kFadeOutDistanceInMeters = 2.; +constexpr float kAlphaUpdateThreshold = 0.2f; + +std::string GetSubmapIdentifier(const int trajectory_id, const int submap_id) { + return std::to_string(trajectory_id) + "-" + std::to_string(submap_id); +} + +} // namespace + +DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id, + ::rviz::FrameManager* const frame_manager, + Ogre::SceneManager* const scene_manager) + : frame_manager_(frame_manager), + scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()), + manual_object_(scene_manager->createManualObject( + kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))), + submap_id_(submap_id), + trajectory_id_(trajectory_id), + last_query_timestamp_(0), + query_in_progress_(false), + texture_count_(0), + current_alpha_(0) { + material_ = Ogre::MaterialManager::getSingleton().getByName( + kSubmapSourceMaterialName); + material_ = material_->clone(kSubmapMaterialPrefix + + GetSubmapIdentifier(trajectory_id_, submap_id)); + material_->setReceiveShadows(false); + material_->getTechnique(0)->setLightingEnabled(false); + material_->setDepthWriteEnabled(false); + scene_node_->attachObject(manual_object_); + connect(this, SIGNAL(RequestSucceeded()), this, SLOT(OnRequestSuccess())); +} + +DrawableSubmap::~DrawableSubmap() { + Ogre::MaterialManager::getSingleton().remove(material_->getHandle()); + if (!texture_.isNull()) { + Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); + texture_.setNull(); + } +} + +bool DrawableSubmap::Update( + const ::google_cartographer_msgs::SubmapEntry& metadata, + ros::ServiceClient* const client) { + ::cartographer::common::MutexLocker locker(&mutex_); + tf::poseMsgToEigen(metadata.pose, submap_pose_); + const bool newer_version_available = version_ < metadata.submap_version; + const std::chrono::milliseconds now = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + const bool recently_queried = + last_query_timestamp_ + kMinQueryDelayInMs > now; + if (!newer_version_available || recently_queried || query_in_progress_) { + return false; + } + query_in_progress_ = true; + last_query_timestamp_ = now; + last_query_slice_height_ = metadata.pose.position.z; + QuerySubmap(submap_id_, trajectory_id_, client); + return true; +} + +void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id, + ros::ServiceClient* const client) { + rpc_request_future_ = std::async( + std::launch::async, [this, submap_id, trajectory_id, client]() { + ::google_cartographer_msgs::SubmapQuery srv; + srv.request.submap_id = submap_id; + srv.request.trajectory_id = trajectory_id; + if (client->call(srv)) { + response_.reset(new ::google_cartographer_msgs::SubmapQuery::Response( + srv.response)); + Q_EMIT RequestSucceeded(); + } else { + OnRequestFailure(); + } + }); +} + +void DrawableSubmap::OnRequestSuccess() { + ::cartographer::common::MutexLocker locker(&mutex_); + version_ = response_->submap_version; + resolution_ = response_->resolution; + width_ = response_->width; + height_ = response_->height; + slice_height_ = last_query_slice_height_; + std::string compressed_cells(response_->cells.begin(), + response_->cells.end()); + cells_.clear(); + ::cartographer::common::FastGunzipString(compressed_cells, &cells_); + Eigen::Affine3d slice_pose; + tf::poseMsgToEigen(response_->slice_pose, slice_pose); + tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_); + response_.reset(); + query_in_progress_ = false; + UpdateSceneNode(); +} + +void DrawableSubmap::OnRequestFailure() { + ::cartographer::common::MutexLocker locker(&mutex_); + query_in_progress_ = false; +} + +bool DrawableSubmap::QueryInProgress() { + ::cartographer::common::MutexLocker locker(&mutex_); + return query_in_progress_; +} + +void DrawableSubmap::UpdateSceneNode() { + // The call to Ogre's loadRawData below does not work with an RG texture, + // therefore we create an RGB one whose blue channel is always 0. + std::vector rgb; + for (int i = 0; i < height_; ++i) { + for (int j = 0; j < width_; ++j) { + auto r = cells_[(i * width_ + j) * 2]; + auto g = cells_[(i * width_ + j) * 2 + 1]; + rgb.push_back(r); + rgb.push_back(g); + rgb.push_back(0.); + } + } + + manual_object_->clear(); + const float metric_width = resolution_ * width_; + const float metric_height = resolution_ * height_; + + manual_object_->begin(material_->getName(), + Ogre::RenderOperation::OT_TRIANGLE_LIST); + { + { + // Bottom left + manual_object_->position(-metric_height, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Bottom right + manual_object_->position(-metric_height, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Bottom right + manual_object_->position(-metric_height, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top right + manual_object_->position(0.0f, -metric_width, 0.0f); + manual_object_->textureCoord(1.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + } + } + + manual_object_->end(); + + Ogre::DataStreamPtr pixel_stream; + pixel_stream.bind(new Ogre::MemoryDataStream(rgb.data(), rgb.size())); + + if (!texture_.isNull()) { + Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); + texture_.setNull(); + } + const std::string texture_name = + kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_) + + std::to_string(texture_count_); + texture_ = Ogre::TextureManager::getSingleton().loadRawData( + texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + pixel_stream, width_, height_, Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0); + ++texture_count_; + + Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0); + pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA); + Ogre::TextureUnitState* const texture_unit = + pass->getNumTextureUnitStates() > 0 ? pass->getTextureUnitState(0) + : pass->createTextureUnitState(); + + texture_unit->setTextureName(texture_->getName()); + texture_unit->setTextureFiltering(Ogre::TFO_NONE); +} + +void DrawableSubmap::Transform(const ros::Time& ros_time) { + Ogre::Vector3 position; + Ogre::Quaternion orientation; + frame_manager_->transform(kMapFrame, ros_time, transformed_pose_, position, + orientation); + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); +} + +void DrawableSubmap::SetAlpha(const double current_tracking_z) { + const double distance_z = std::abs(slice_height_ - current_tracking_z); + const double fade_distance = + std::max(distance_z - kFadeOutStartDistanceInMeters, 0.); + const float alpha = + (float)std::max(0., 1. - fade_distance / kFadeOutDistanceInMeters); + + const Ogre::GpuProgramParametersSharedPtr parameters = + material_->getTechnique(0)->getPass(0)->getFragmentProgramParameters(); + parameters->setNamedConstant("u_alpha", UpdateAlpha(alpha)); +} + +float DrawableSubmap::UpdateAlpha(const float target_alpha) { + if (std::abs(target_alpha - current_alpha_) > kAlphaUpdateThreshold || + target_alpha == 0.f || target_alpha == 1.f) { + current_alpha_ = target_alpha; + } + return current_alpha_; +} + +} // namespace rviz +} // namespace cartographer_ros diff --git a/ros/google_cartographer/src/drawable_submap.h b/ros/google_cartographer/src/drawable_submap.h new file mode 100644 index 0000000..cbdd53f --- /dev/null +++ b/ros/google_cartographer/src/drawable_submap.h @@ -0,0 +1,117 @@ +/* + * 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_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_ +#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace cartographer_ros { +namespace rviz { + +// Contains all the information needed to render a submap onto the final +// texture representing the whole map. +class DrawableSubmap : public QObject { + Q_OBJECT + + public: + // Each submap is identified by a 'trajectory_id' plus a 'submap_id'. The + // 'frame_manager' is needed to transform the scene node before rendering + // onto the offscreen texture. 'scene_manager' is the Ogre scene manager which + // contains all submaps. + DrawableSubmap(int submap_id, int trajectory_id, + ::rviz::FrameManager* frame_manager, + Ogre::SceneManager* scene_manager); + ~DrawableSubmap() override; + DrawableSubmap(const DrawableSubmap&) = delete; + DrawableSubmap& operator=(const DrawableSubmap&) = delete; + + // 'submap_entry' contains metadata which is used to find out whether this + // 'DrawableSubmap' should update itself. If an update is needed, it will send + // an RPC using 'client' to request the new data for the submap. + bool Update(const ::google_cartographer_msgs::SubmapEntry& submap_entry, + ros::ServiceClient* client); + + // Returns whether an RPC is in progress. + bool QueryInProgress(); + + // Transforms the scene node for this submap before being rendered onto a + // texture. + void Transform(const ros::Time& ros_time); + + // Sets the alpha of the submap taking into account its slice height and the + // 'current_tracking_z'. + void SetAlpha(double current_tracking_z); + + Q_SIGNALS: + // RPC request succeeded. + void RequestSucceeded(); + + private Q_SLOTS: + // Callback when an rpc request succeeded. + void OnRequestSuccess(); + + private: + void QuerySubmap(int submap_id, int trajectory_id, + ros::ServiceClient* client); + void OnRequestFailure(); + void UpdateSceneNode(); + float UpdateAlpha(float target_alpha); + + const int submap_id_; + const int trajectory_id_; + + ::cartographer::common::Mutex mutex_; + ::rviz::FrameManager* frame_manager_; + Ogre::SceneNode* const scene_node_; + Ogre::ManualObject* manual_object_; + Ogre::TexturePtr texture_; + Ogre::MaterialPtr material_; + Eigen::Affine3d submap_pose_ GUARDED_BY(mutex_); + geometry_msgs::Pose transformed_pose_ GUARDED_BY(mutex_); + std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); + bool query_in_progress_ GUARDED_BY(mutex_); + float resolution_ GUARDED_BY(mutex_); + int width_ GUARDED_BY(mutex_); + int height_ GUARDED_BY(mutex_); + int version_ GUARDED_BY(mutex_); + double slice_height_ GUARDED_BY(mutex_); + double last_query_slice_height_ GUARDED_BY(mutex_); + std::future rpc_request_future_; + std::string cells_ GUARDED_BY(mutex_); + std::unique_ptr<::google_cartographer_msgs::SubmapQuery::Response> response_ + GUARDED_BY(mutex_); + int texture_count_; + float current_alpha_; +}; + +} // namespace rviz +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_DRAWABLE_SUBMAP_H_ diff --git a/ros/google_cartographer/src/msg_conversion.cc b/ros/google_cartographer/src/msg_conversion.cc new file mode 100644 index 0000000..31a18e4 --- /dev/null +++ b/ros/google_cartographer/src/msg_conversion.cc @@ -0,0 +1,261 @@ +/* + * 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 "msg_conversion.h" + +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/proto/transform.pb.h" +#include "cartographer/transform/transform.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/TransformStamped.h" +#include "geometry_msgs/Vector3.h" +#include "glog/logging.h" +#include "ros/ros.h" +#include "ros/serialization.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/PointCloud2.h" + +#include "time_conversion.h" + +namespace cartographer_ros { +namespace { + +// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each +// point. The last one must be this value or RViz is not showing the point cloud +// properly. +constexpr float kPointCloudComponentFourMagic = 1.; + +void ToMessage(const ::cartographer::transform::proto::Vector3d& proto, + geometry_msgs::Vector3* vector) { + vector->x = proto.x(); + vector->y = proto.y(); + vector->z = proto.z(); +} + +void ToMessage(const ::cartographer::transform::proto::Quaterniond& proto, + geometry_msgs::Quaternion* quaternion) { + quaternion->w = proto.w(); + quaternion->x = proto.x(); + quaternion->y = proto.y(); + quaternion->z = proto.z(); +} + +} // namespace + +sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage( + const int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::LaserScan& laser_scan) { + sensor_msgs::MultiEchoLaserScan msg; + msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); + msg.header.frame_id = frame_id; + + msg.angle_min = laser_scan.angle_min(); + msg.angle_max = laser_scan.angle_max(); + msg.angle_increment = laser_scan.angle_increment(); + msg.time_increment = laser_scan.time_increment(); + msg.scan_time = laser_scan.scan_time(); + msg.range_min = laser_scan.range_min(); + msg.range_max = laser_scan.range_max(); + + for (const auto& echoes : laser_scan.range()) { + msg.ranges.emplace_back(); + std::copy(echoes.value().begin(), echoes.value().end(), + std::back_inserter(msg.ranges.back().echoes)); + } + + for (const auto& echoes : laser_scan.intensity()) { + msg.intensities.emplace_back(); + std::copy(echoes.value().begin(), echoes.value().end(), + std::back_inserter(msg.intensities.back().echoes)); + } + return msg; +} + +sensor_msgs::Imu ToImuMessage(const int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::Imu& imu) { + sensor_msgs::Imu message; + message.header.stamp = + ToRos(::cartographer::common::FromUniversal(timestamp)); + message.header.frame_id = frame_id; + + ToMessage(imu.orientation(), &message.orientation); + message.orientation_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}}; + ToMessage(imu.angular_velocity(), &message.angular_velocity); + message.angular_velocity_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}}; + ToMessage(imu.linear_acceleration(), &message.linear_acceleration); + message.linear_acceleration_covariance = { + {0., 0., 0., 0., 0., 0., 0., 0., 0.}}; + return message; +} + +sensor_msgs::LaserScan ToLaserScan( + const int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::LaserScan& laser_scan) { + sensor_msgs::LaserScan msg; + msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); + msg.header.frame_id = frame_id; + + msg.angle_min = laser_scan.angle_min(); + msg.angle_max = laser_scan.angle_max(); + msg.angle_increment = laser_scan.angle_increment(); + msg.time_increment = laser_scan.time_increment(); + msg.scan_time = laser_scan.scan_time(); + msg.range_min = laser_scan.range_min(); + msg.range_max = laser_scan.range_max(); + + for (const auto& echoes : laser_scan.range()) { + if (echoes.value_size() > 0) { + msg.ranges.push_back(echoes.value(0)); + } else { + msg.ranges.push_back(0.); + } + } + + bool has_intensities = false; + for (const auto& echoes : laser_scan.intensity()) { + if (echoes.value_size() > 0) { + msg.intensities.push_back(echoes.value(0)); + has_intensities = true; + } else { + msg.intensities.push_back(0); + } + } + if (!has_intensities) { + msg.intensities.clear(); + } + + return msg; +} + +sensor_msgs::PointCloud2 ToPointCloud2Message( + const int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d) { + CHECK(::cartographer::transform::ToEigen(laser_scan_3d.origin()).norm() == 0) + << "Trying to convert a laser_scan_3d that is not at the origin."; + + sensor_msgs::PointCloud2 msg; + msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); + msg.header.frame_id = frame_id; + + const auto& point_cloud = laser_scan_3d.point_cloud(); + CHECK_EQ(point_cloud.x_size(), point_cloud.y_size()); + CHECK_EQ(point_cloud.x_size(), point_cloud.z_size()); + const auto num_points = point_cloud.x_size(); + + msg.height = 1; + msg.width = num_points; + msg.fields.resize(3); + msg.fields[0].name = "x"; + msg.fields[0].offset = 0; + msg.fields[0].datatype = 7; + msg.fields[0].count = 1; + msg.fields[1].name = "y"; + msg.fields[1].offset = 4; + msg.fields[1].datatype = 7; + msg.fields[1].count = 1; + msg.fields[2].name = "z"; + msg.fields[2].offset = 8; + msg.fields[2].datatype = 7; + msg.fields[2].count = 1; + msg.is_bigendian = false; + msg.point_step = 16; + msg.row_step = 16 * msg.width; + msg.is_dense = true; + msg.data.resize(16 * num_points); + + ::ros::serialization::OStream stream(msg.data.data(), msg.data.size()); + for (int i = 0; i < num_points; ++i) { + stream.next(point_cloud.x(i)); + stream.next(point_cloud.y(i)); + stream.next(point_cloud.z(i)); + stream.next(kPointCloudComponentFourMagic); + } + return msg; +} + +::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg) { + ::cartographer::sensor::proto::Imu proto; + + if (msg.orientation_covariance[0] != -1) { + auto* orientation = proto.mutable_orientation(); + orientation->set_x(msg.orientation.x); + orientation->set_y(msg.orientation.y); + orientation->set_z(msg.orientation.z); + orientation->set_w(msg.orientation.w); + } + + if (msg.angular_velocity_covariance[0] != -1) { + auto* angular_velocity = proto.mutable_angular_velocity(); + angular_velocity->set_x(msg.angular_velocity.x); + angular_velocity->set_y(msg.angular_velocity.y); + angular_velocity->set_z(msg.angular_velocity.z); + } + + if (msg.linear_acceleration_covariance[0] != -1) { + auto* linear_acceleration = proto.mutable_linear_acceleration(); + linear_acceleration->set_x(msg.linear_acceleration.x); + linear_acceleration->set_y(msg.linear_acceleration.y); + linear_acceleration->set_z(msg.linear_acceleration.z); + } + return proto; +} + +::cartographer::sensor::proto::LaserScan ToCartographer( + const sensor_msgs::LaserScan& msg) { + ::cartographer::sensor::proto::LaserScan proto; + proto.set_angle_min(msg.angle_min); + proto.set_angle_max(msg.angle_max); + proto.set_angle_increment(msg.angle_increment); + proto.set_time_increment(msg.time_increment); + proto.set_scan_time(msg.scan_time); + proto.set_range_min(msg.range_min); + proto.set_range_max(msg.range_max); + + for (const auto& range : msg.ranges) { + proto.add_range()->mutable_value()->Add(range); + } + + for (const auto& intensity : msg.intensities) { + proto.add_intensity()->mutable_value()->Add(intensity); + } + return proto; +} + +::cartographer::sensor::proto::LaserFan3D ToCartographer( + const pcl::PointCloud& pcl_points) { + ::cartographer::sensor::proto::LaserFan3D proto; + + auto* origin = proto.mutable_origin(); + origin->set_x(0.); + origin->set_y(0.); + origin->set_z(0.); + + auto* point_cloud = proto.mutable_point_cloud(); + for (const auto& point : pcl_points) { + point_cloud->add_x(point.x); + point_cloud->add_y(point.y); + point_cloud->add_z(point.z); + } + return proto; +} + +} // namespace cartographer_ros diff --git a/ros/google_cartographer/src/msg_conversion.h b/ros/google_cartographer/src/msg_conversion.h new file mode 100644 index 0000000..f553d04 --- /dev/null +++ b/ros/google_cartographer/src/msg_conversion.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_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_ +#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_ + +#include "cartographer/common/port.h" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/PointCloud2.h" + +namespace cartographer_ros { + +// Only uses first echo of each beam. +sensor_msgs::LaserScan ToLaserScan( + int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::LaserScan& laser_scan); + +sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage( + int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::LaserScan& laser_scan); + +sensor_msgs::Imu ToImuMessage(int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::Imu& imu); + +sensor_msgs::PointCloud2 ToPointCloud2Message( + int64 timestamp, const string& frame_id, + const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d); + +::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg); + +::cartographer::sensor::proto::LaserScan ToCartographer( + const sensor_msgs::LaserScan& msg); + +::cartographer::sensor::proto::LaserFan3D ToCartographer( + const pcl::PointCloud& pcl_points); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_MSG_CONVERSION_H_ diff --git a/ros/google_cartographer/src/node_constants.h b/ros/google_cartographer/src/node_constants.h new file mode 100644 index 0000000..84c3330 --- /dev/null +++ b/ros/google_cartographer/src/node_constants.h @@ -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. + */ + +#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_ +#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_ + +namespace cartographer_ros { + +// The topic that the node will subscrbe under. +constexpr char kSubmapListTopic[] = "submap_list"; + +// The service we serve in the Node and query in the RViz plugin for submap +// which are used for visualization. +constexpr char kSubmapQueryServiceName[] = "submap_query"; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_NODE_CONSTANTS_H_ diff --git a/ros/google_cartographer/src/submaps_display.cc b/ros/google_cartographer/src/submaps_display.cc new file mode 100644 index 0000000..10e24f4 --- /dev/null +++ b/ros/google_cartographer/src/submaps_display.cc @@ -0,0 +1,331 @@ +/* + * 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 "submaps_display.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cartographer_ros { +namespace rviz { + +namespace { + +constexpr int kMaxOnGoingRequests = 6; +constexpr char kMaterialsDirectory[] = "/ogre_media/materials"; +constexpr char kGlsl120Directory[] = "/glsl120"; +constexpr char kScriptsDirectory[] = "/scripts"; +constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial"; +constexpr char kScreenBlitSourceMaterialName[] = + "google_cartographer/ScreenBlit"; +constexpr char kSubmapsRttPrefix[] = "SubmapsRtt"; +constexpr char kMapTextureName[] = "MapTexture"; +constexpr char kMapOverlayName[] = "MapOverlay"; +constexpr char kSubmapsSceneCameraName[] = "SubmapsSceneCamera"; +constexpr char kSubmapTexturesGroup[] = "SubmapTexturesGroup"; +constexpr char kDefaultMapFrame[] = "map"; +constexpr char kDefaultTrackingFrame[] = "base_link"; + +} // namespace + +SubmapsDisplay::SubmapsDisplay() + : Display(), + rtt_count_(0), + scene_manager_listener_([this]() { UpdateMapTexture(); }), + tf_listener_(tf_buffer_) { + connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps())); + topic_property_ = new ::rviz::RosTopicProperty( + "Topic", "", + QString::fromStdString(ros::message_traits::datatype< + ::google_cartographer_msgs::SubmapList>()), + "google_cartographer_msgs::SubmapList topic to subscribe to.", this, + SLOT(UpdateTopic())); + submap_query_service_property_ = new ::rviz::StringProperty( + "Submap query service", "", "Submap query service to connect to.", this, + SLOT(UpdateSubmapQueryServiceName())); + map_frame_property_ = new ::rviz::StringProperty( + "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.", + this); + tracking_frame_property_ = new ::rviz::StringProperty( + "Tracking frame", kDefaultTrackingFrame, + "Tracking frame, used for fading out submaps.", this); + client_ = + update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>(""); + const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().addResourceLocation( + package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().addResourceLocation( + package_path + kMaterialsDirectory + kGlsl120Directory, "FileSystem", + ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().addResourceLocation( + package_path + kMaterialsDirectory + kScriptsDirectory, "FileSystem", + ROS_PACKAGE_NAME); + Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); +} + +SubmapsDisplay::~SubmapsDisplay() { + Unsubscribe(); + client_.shutdown(); + Clear(); +} + +void SubmapsDisplay::onInitialize() { + submaps_scene_manager_ = + Ogre::Root::getSingletonPtr()->createSceneManager(Ogre::ST_GENERIC); + submaps_scene_camera_ = + submaps_scene_manager_->createCamera(kSubmapsSceneCameraName); + submap_scene_material_ = Ogre::MaterialManager::getSingleton().create( + kMapTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + + screen_blit_material_ = Ogre::MaterialManager::getSingleton().getByName( + kScreenBlitSourceMaterialName); + screen_blit_material_ = screen_blit_material_->clone(kScreenBlitMaterialName); + screen_blit_material_->setReceiveShadows(false); + screen_blit_material_->getTechnique(0)->setLightingEnabled(false); + screen_blit_material_->setDepthWriteEnabled(false); + + Ogre::OverlayManager& overlay_manager = Ogre::OverlayManager::getSingleton(); + overlay_ = overlay_manager.create(kMapOverlayName); + panel_ = static_cast( + overlay_manager.createOverlayElement("Panel", "PanelName")); + overlay_->add2D(panel_); + panel_->setPosition(0.0, 0.0); + panel_->setDimensions(1., 1.); + panel_->setMaterialName(kScreenBlitMaterialName); + + Ogre::ResourceGroupManager::getSingleton().createResourceGroup( + kSubmapTexturesGroup); + + scene_manager_->addListener(&scene_manager_listener_); + UpdateTopic(); +} + +void SubmapsDisplay::UpdateTopic() { + Unsubscribe(); + Clear(); + Subscribe(); +} + +void SubmapsDisplay::UpdateSubmapQueryServiceName() { + Unsubscribe(); + Clear(); + client_.shutdown(); + client_ = update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>( + submap_query_service_property_->getStdString()); + Subscribe(); +} + +void SubmapsDisplay::reset() { + Display::reset(); + + Clear(); + UpdateTopic(); +} + +void SubmapsDisplay::onEnable() { Subscribe(); } + +void SubmapsDisplay::onDisable() { + Unsubscribe(); + Clear(); +} + +void SubmapsDisplay::Subscribe() { + if (!isEnabled()) { + return; + } + + if (!topic_property_->getTopic().isEmpty()) { + try { + submap_list_subscriber_ = + update_nh_.subscribe(topic_property_->getTopicStd(), 1, + &SubmapsDisplay::IncomingSubmapList, this, + ros::TransportHints().reliable()); + setStatus(::rviz::StatusProperty::Ok, "Topic", "OK"); + } catch (ros::Exception& e) { + setStatus(::rviz::StatusProperty::Error, "Topic", + QString("Error subscribing: ") + e.what()); + } + } +} + +void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); } + +void SubmapsDisplay::IncomingSubmapList( + const ::google_cartographer_msgs::SubmapList::ConstPtr& msg) { + submap_list_ = *msg; + Q_EMIT SubmapListUpdated(); +} + +void SubmapsDisplay::Clear() { + ::cartographer::common::MutexLocker locker(&mutex_); + submaps_scene_manager_->clearScene(); + if (!rttTexture_.isNull()) { + rttTexture_->unload(); + rttTexture_.setNull(); + } + Ogre::ResourceGroupManager::getSingleton().clearResourceGroup( + kSubmapTexturesGroup); + trajectories_.clear(); + overlay_->hide(); +} + +void SubmapsDisplay::RequestNewSubmaps() { + ::cartographer::common::MutexLocker locker(&mutex_); + for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size(); + ++trajectory_id) { + if (trajectory_id >= trajectories_.size()) { + trajectories_.emplace_back(new Trajectory); + } + const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries = + submap_list_.trajectory[trajectory_id].submap; + if (submap_entries.empty()) { + return; + } + for (int submap_id = trajectories_[trajectory_id]->Size(); + submap_id < submap_entries.size(); ++submap_id) { + trajectories_[trajectory_id]->Add(submap_id, trajectory_id, + context_->getFrameManager(), + submaps_scene_manager_); + } + } + int num_ongoing_requests = 0; + for (const auto& trajectory : trajectories_) { + for (int i = 0; i < trajectory->Size(); ++i) { + if (trajectory->Get(i).QueryInProgress()) { + ++num_ongoing_requests; + if (num_ongoing_requests == kMaxOnGoingRequests) { + return; + } + } + } + } + for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size(); + ++trajectory_id) { + const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries = + submap_list_.trajectory[trajectory_id].submap; + for (int submap_id = submap_entries.size() - 1; submap_id >= 0; + --submap_id) { + if (trajectories_[trajectory_id]->Get(submap_id).Update( + submap_entries[submap_id], &client_)) { + ++num_ongoing_requests; + if (num_ongoing_requests == kMaxOnGoingRequests) { + return; + } + } + } + } +} + +void SubmapsDisplay::UpdateMapTexture() { + if (trajectories_.empty()) { + return; + } + const int width = scene_manager_->getCurrentViewport()->getActualWidth(); + const int height = scene_manager_->getCurrentViewport()->getActualHeight(); + if (!rttTexture_.isNull()) { + rttTexture_->unload(); + rttTexture_.setNull(); + } + // If the rtt texture is freed every time UpdateMapTexture() is called, the + // code slows down a lot. Therefore, we assign them to a group and free them + // every 100th texture. + if (rtt_count_ % 100 == 0) { + Ogre::ResourceGroupManager::getSingleton().clearResourceGroup( + kSubmapTexturesGroup); + } + rttTexture_ = + Ogre::Root::getSingletonPtr()->getTextureManager()->createManual( + kSubmapsRttPrefix + std::to_string(rtt_count_), kSubmapTexturesGroup, + Ogre::TEX_TYPE_2D, width, height, 0, Ogre::PF_RG8, + Ogre::TU_RENDERTARGET); + rtt_count_++; + + Ogre::Pass* rtt_pass = submap_scene_material_->getTechnique(0)->getPass(0); + Ogre::TextureUnitState* const rtt_tex_unit = + rtt_pass->getNumTextureUnitStates() > 0 + ? rtt_pass->getTextureUnitState(0) + : rtt_pass->createTextureUnitState(); + rtt_tex_unit->setTexture(rttTexture_); + + Ogre::RenderTexture* const renderTexture = + rttTexture_->getBuffer()->getRenderTarget(); + renderTexture->addViewport(submaps_scene_camera_) + ->setBackgroundColour(Ogre::ColourValue(0.5f, 0.f, 0.f)); + { + ::cartographer::common::MutexLocker locker(&mutex_); + // TODO(pedrofernandez): Add support for more than one trajectory. + for (int i = 0; i < trajectories_.front()->Size(); ++i) { + trajectories_.front()->Get(i).Transform(ros::Time()); + try { + const ::geometry_msgs::TransformStamped transform_stamped = + tf_buffer_.lookupTransform(map_frame_property_->getStdString(), + tracking_frame_property_->getStdString(), + ros::Time(0) /* latest */); + trajectories_.front()->Get(i).SetAlpha( + transform_stamped.transform.translation.z); + } catch (tf2::TransformException& e) { + ROS_WARN("Could not compute submap fading: %s", e.what()); + } + } + } + Ogre::Camera* const actual_camera = + scene_manager_->getCurrentViewport()->getCamera(); + submaps_scene_camera_->synchroniseBaseSettingsWith(actual_camera); + submaps_scene_camera_->setCustomProjectionMatrix( + true, actual_camera->getProjectionMatrix()); + renderTexture->update(); + + Ogre::Pass* const pass = screen_blit_material_->getTechnique(0)->getPass(0); + pass->setSceneBlending(Ogre::SBF_SOURCE_ALPHA, + Ogre::SBF_ONE_MINUS_SOURCE_ALPHA); + Ogre::TextureUnitState* const tex_unit = pass->getNumTextureUnitStates() > 0 + ? pass->getTextureUnitState(0) + : pass->createTextureUnitState(); + tex_unit->setTextureName(rttTexture_->getName()); + tex_unit->setTextureFiltering(Ogre::TFO_NONE); + overlay_->show(); +} + +} // namespace rviz +} // namespace cartographer_ros + +PLUGINLIB_EXPORT_CLASS(cartographer_ros::rviz::SubmapsDisplay, ::rviz::Display) diff --git a/ros/google_cartographer/src/submaps_display.h b/ros/google_cartographer/src/submaps_display.h new file mode 100644 index 0000000..f3e85e6 --- /dev/null +++ b/ros/google_cartographer/src/submaps_display.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_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_ +#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "drawable_submap.h" +#include "trajectory.h" + +namespace cartographer_ros { +namespace rviz { + +// RViz plugin used for displaying maps which are represented by a collection of +// submaps. +// +// We keep a separate Ogre scene to the one provided by rviz and in it we place +// every submap as a SceneNode. We show an X-ray view of the map which is +// achieved by shipping textures for every submap containing pre-multiplied +// alpha and grayscale values, these are then alpha blended together onto an +// offscreen texture. This offscreen texture is then screen blit onto the screen +// as a grayscale image. +class SubmapsDisplay : public ::rviz::Display { + Q_OBJECT + + public: + SubmapsDisplay(); + ~SubmapsDisplay() override; + + SubmapsDisplay(const SubmapsDisplay&) = delete; + SubmapsDisplay& operator=(const SubmapsDisplay&) = delete; + + // Called by RViz on initialization of the plugin. + void onInitialize() override; + // Called to tell the display to clear its state. + void reset() override; + + Q_SIGNALS: + void SubmapListUpdated(); + + private Q_SLOTS: + void RequestNewSubmaps(); + void UpdateTopic(); + void UpdateSubmapQueryServiceName(); + + private: + class SceneManagerListener : public Ogre::SceneManager::Listener { + public: + SceneManagerListener(std::function callback) + : callback_(callback) {} + void preUpdateSceneGraph(Ogre::SceneManager* source, Ogre::Camera* camera) { + callback_(); + } + + private: + std::function callback_; + }; + + void onEnable() override; + void onDisable() override; + void Subscribe(); + void Unsubscribe(); + void UpdateMapTexture(); + void IncomingSubmapList( + const ::google_cartographer_msgs::SubmapList::ConstPtr& msg); + // Clears the current map. + void Clear(); + void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg); + + int rtt_count_; + SceneManagerListener scene_manager_listener_; + ::google_cartographer_msgs::SubmapList submap_list_; + ros::Subscriber submap_list_subscriber_; + ::tf2_ros::Buffer tf_buffer_; + ::tf2_ros::TransformListener tf_listener_; + ros::ServiceClient client_; + ::rviz::RosTopicProperty* topic_property_; + ::rviz::StringProperty* submap_query_service_property_; + ::rviz::StringProperty* map_frame_property_; + ::rviz::StringProperty* tracking_frame_property_; + std::vector> trajectories_ GUARDED_BY(mutex_); + Ogre::SceneManager* submaps_scene_manager_; + Ogre::Camera* submaps_scene_camera_; + Ogre::MaterialPtr submap_scene_material_; + Ogre::MaterialPtr screen_blit_material_; + Ogre::Overlay* overlay_; + Ogre::OverlayContainer* panel_; + Ogre::TexturePtr rttTexture_; + ::cartographer::common::Mutex mutex_; +}; + +} // namespace rviz +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_SUBMAPS_DISPLAY_H_ diff --git a/ros/google_cartographer/src/time_conversion.cc b/ros/google_cartographer/src/time_conversion.cc new file mode 100644 index 0000000..7dfda18 --- /dev/null +++ b/ros/google_cartographer/src/time_conversion.cc @@ -0,0 +1,43 @@ +/* + * 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 "time_conversion.h" + +#include "cartographer/common/time.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +::ros::Time ToRos(::cartographer::common::Time time) { + int64 uts_timestamp = ::cartographer::common::ToUniversal(time); + int64 ns_since_unix_epoch = + (uts_timestamp - kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll) * + 100ll; + ::ros::Time ros_time; + ros_time.fromNSec(ns_since_unix_epoch); + return ros_time; +} + +// TODO(pedrofernandez): Write test. +::cartographer::common::Time FromRos(const ::ros::Time& time) { + // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000", + // exactly 719162 days before the Unix epoch. + return ::cartographer::common::FromUniversal( + (time.sec + kUtsEpochOffsetFromUnixEpochInSeconds) * 10000000ll + + (time.nsec + 50) / 100); // + 50 to get the rounding correct. +} + +} // namespace cartographer_ros diff --git a/ros/google_cartographer/src/time_conversion.h b/ros/google_cartographer/src/time_conversion.h new file mode 100644 index 0000000..863d871 --- /dev/null +++ b/ros/google_cartographer/src/time_conversion.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_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_ +#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_ + +#include "cartographer/common/time.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds = + (719162ll * 24ll * 60ll * 60ll); + +::ros::Time ToRos(::cartographer::common::Time time); + +::cartographer::common::Time FromRos(const ::ros::Time& time); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TIME_CONVERSION_H_ diff --git a/ros/google_cartographer/src/time_conversion_test.cc b/ros/google_cartographer/src/time_conversion_test.cc new file mode 100644 index 0000000..693a67b --- /dev/null +++ b/ros/google_cartographer/src/time_conversion_test.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 "time_conversion.h" + +#include + +#include "cartographer/common/time.h" +#include "gtest/gtest.h" +#include "ros/ros.h" + +namespace cartographer_ros { + +namespace { + +TEST(TimeConversion, testToRos) { + std::vector values = {0, 1469091375, 1466481821, 1462101382, + 1468238899}; + for (int64 seconds_since_epoch : values) { + ::ros::Time ros_now; + ros_now.fromSec(seconds_since_epoch); + ::cartographer::common::Time cartographer_now( + ::cartographer::common::FromSeconds( + seconds_since_epoch + kUtsEpochOffsetFromUnixEpochInSeconds)); + EXPECT_EQ(cartographer_now, ::cartographer_ros::FromRos(ros_now)); + EXPECT_EQ(ros_now, ::cartographer_ros::ToRos(cartographer_now)); + } +} + +} // namespace +} // namespace cartographer_ros diff --git a/ros/google_cartographer/src/trajectory.cc b/ros/google_cartographer/src/trajectory.cc new file mode 100644 index 0000000..995a605 --- /dev/null +++ b/ros/google_cartographer/src/trajectory.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 "trajectory.h" + +#include +#include +#include +#include + +#include +#include +#include + +#include "drawable_submap.h" + +namespace cartographer_ros { +namespace rviz { + +void Trajectory::Add(const int submap_id, const int trajectory_id, + ::rviz::FrameManager* const frame_manager, + Ogre::SceneManager* const scene_manager) { + std::lock_guard guard(mutex_); + drawable_submaps_.push_back( + ::cartographer::common::make_unique( + submap_id, trajectory_id, frame_manager, scene_manager)); +} + +DrawableSubmap& Trajectory::Get(const int index) { + std::lock_guard guard(mutex_); + return *drawable_submaps_[index]; +} + +int Trajectory::Size() { return drawable_submaps_.size(); } + +} // namespace rviz +} // namespace cartographer_ros diff --git a/ros/google_cartographer/src/trajectory.h b/ros/google_cartographer/src/trajectory.h new file mode 100644 index 0000000..12ff234 --- /dev/null +++ b/ros/google_cartographer/src/trajectory.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_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ +#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ + +#include +#include +#include + +#include +#include +#include + +#include "drawable_submap.h" + +namespace cartographer_ros { +namespace rviz { + +// Contains a list of drawable submaps. +class Trajectory { + public: + Trajectory() = default; + + Trajectory(const Trajectory&) = delete; + Trajectory& operator=(const Trajectory&) = delete; + + // Creates a new DrawableSubmap and stores it as part of this trajectory. + void Add(int submap_id, int trajectory_id, + ::rviz::FrameManager* frame_manager, + Ogre::SceneManager* scene_manager); + // Gets the submap at 'index' and returns a non-const reference. + DrawableSubmap& Get(int index); + // Returns the number of DrawableSubmaps this trajectory contains. + int Size(); + + private: + std::mutex mutex_; + std::vector> drawable_submaps_; +}; + +} // namespace rviz +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_ diff --git a/ros/google_cartographer/urdf/backpack_2d.urdf b/ros/google_cartographer/urdf/backpack_2d.urdf new file mode 100644 index 0000000..e72be50 --- /dev/null +++ b/ros/google_cartographer/urdf/backpack_2d.urdf @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/google_cartographer/urdf/backpack_3d.urdf b/ros/google_cartographer/urdf/backpack_3d.urdf new file mode 100644 index 0000000..89f32ef --- /dev/null +++ b/ros/google_cartographer/urdf/backpack_3d.urdf @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/google_cartographer/urdf/turtlebot.urdf b/ros/google_cartographer/urdf/turtlebot.urdf new file mode 100644 index 0000000..f97dcb7 --- /dev/null +++ b/ros/google_cartographer/urdf/turtlebot.urdf @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/google_cartographer_msgs/CMakeLists.txt b/ros/google_cartographer_msgs/CMakeLists.txt new file mode 100644 index 0000000..9551707 --- /dev/null +++ b/ros/google_cartographer_msgs/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. + +cmake_minimum_required(VERSION 2.8.3) +project(google_cartographer_msgs) +set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") +find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation) + +add_message_files( + FILES + SubmapList.msg + TrajectorySubmapList.msg + SubmapEntry.msg +) + +add_service_files( + FILES + SubmapQuery.srv +) + +generate_messages( + DEPENDENCIES + geometry_msgs +) + +catkin_package() diff --git a/ros/google_cartographer_msgs/msg/SubmapEntry.msg b/ros/google_cartographer_msgs/msg/SubmapEntry.msg new file mode 100644 index 0000000..605de24 --- /dev/null +++ b/ros/google_cartographer_msgs/msg/SubmapEntry.msg @@ -0,0 +1,16 @@ +# 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. + +uint32 submap_version +geometry_msgs/Pose pose diff --git a/ros/google_cartographer_msgs/msg/SubmapList.msg b/ros/google_cartographer_msgs/msg/SubmapList.msg new file mode 100644 index 0000000..dbc7b08 --- /dev/null +++ b/ros/google_cartographer_msgs/msg/SubmapList.msg @@ -0,0 +1,15 @@ +# 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. + +TrajectorySubmapList[] trajectory diff --git a/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg b/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg new file mode 100644 index 0000000..678a886 --- /dev/null +++ b/ros/google_cartographer_msgs/msg/TrajectorySubmapList.msg @@ -0,0 +1,15 @@ +# 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. + +SubmapEntry[] submap diff --git a/ros/google_cartographer_msgs/package.xml b/ros/google_cartographer_msgs/package.xml new file mode 100644 index 0000000..a6d2cad --- /dev/null +++ b/ros/google_cartographer_msgs/package.xml @@ -0,0 +1,29 @@ + + + + google_cartographer_msgs + 1.0.0 + + The google_cartographer_msgs package. + + Google + Apache 2.0 + + catkin + + message_generation + diff --git a/ros/google_cartographer_msgs/srv/SubmapQuery.srv b/ros/google_cartographer_msgs/srv/SubmapQuery.srv new file mode 100644 index 0000000..4463260 --- /dev/null +++ b/ros/google_cartographer_msgs/srv/SubmapQuery.srv @@ -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. + +int32 submap_id +int32 trajectory_id +--- +int32 submap_id +int32 submap_version +uint8[] cells +int32 width +int32 height +float64 resolution +geometry_msgs/Pose slice_pose +string error_message