Initial import of Cartographer codebase.
Damon Kohler
commit
166f2568bc
|
|
@ -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
|
||||
|
|
@ -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).
|
||||
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
# Cartographer Project Overview
|
||||
|
||||
Cartographer is a system that provides real-time simultaneous localization and
|
||||
mapping
|
||||
([SLAM](http://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping))
|
||||
across multiple platforms and sensor configurations.
|
||||
|
||||
## Installation instructions
|
||||
|
||||
For Ubuntu 14.04 (Trusty):
|
||||
|
||||
sudo apt-get install \
|
||||
g++ \
|
||||
google-mock \
|
||||
libboost-all-dev \
|
||||
libgflags-dev \
|
||||
libgoogle-glog-dev \
|
||||
liblua5.2-dev \
|
||||
libprotobuf-dev \
|
||||
libsuitesparse-dev \
|
||||
libwebp-dev \
|
||||
ninja-build \
|
||||
protobuf-compiler \
|
||||
python-sphinx
|
||||
|
||||
Download, build and install Ceres:
|
||||
|
||||
git clone https://ceres-solver.googlesource.com/ceres-solver
|
||||
cd ceres-solver
|
||||
mkdir build && cd build
|
||||
cmake ..
|
||||
make
|
||||
sudo make install
|
||||
|
||||
Build Cartographer:
|
||||
|
||||
cd cartographer
|
||||
mkdir build && cd build
|
||||
cmake .. -G Ninja
|
||||
ninja
|
||||
|
||||
## Running with Velodyne data
|
||||
|
||||
apt-get install libpcap-dev
|
||||
cd <somwhere>
|
||||
git clone git@github.com:ros-drivers/velodyne.git
|
||||
cd <catkin_ws>/src
|
||||
ln -s <somewhere>/velodyne/velodyne* .
|
||||
|
|
@ -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/
|
||||
)
|
||||
|
|
@ -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()
|
||||
|
|
@ -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")
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_
|
||||
#define CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <deque>
|
||||
#include <memory>
|
||||
|
||||
#include "cartographer/common/mutex.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// A thread-safe blocking queue that is useful for producer/consumer patterns.
|
||||
// 'T' must be movable.
|
||||
template <typename T>
|
||||
class BlockingQueue {
|
||||
public:
|
||||
static constexpr size_t kInfiniteQueueSize = 0;
|
||||
|
||||
// Constructs a blocking queue with infinite queue size.
|
||||
BlockingQueue() : BlockingQueue(kInfiniteQueueSize) {}
|
||||
|
||||
BlockingQueue(const BlockingQueue&) = delete;
|
||||
BlockingQueue& operator=(const BlockingQueue&) = delete;
|
||||
|
||||
// Constructs a blocking queue with a size of 'queue_size'.
|
||||
explicit BlockingQueue(const size_t queue_size) : queue_size_(queue_size) {}
|
||||
|
||||
// Pushes a value onto the queue. Blocks if the queue is full.
|
||||
void Push(T t) {
|
||||
MutexLocker lock(&mutex_);
|
||||
lock.Await([this]() REQUIRES(mutex_) { return QueueNotFullCondition(); });
|
||||
deque_.push_back(std::move(t));
|
||||
}
|
||||
|
||||
// Like push, but returns false if 'timeout' is reached.
|
||||
bool PushWithTimeout(T t, const common::Duration timeout) {
|
||||
MutexLocker lock(&mutex_);
|
||||
if (!lock.AwaitWithTimeout(
|
||||
[this]() REQUIRES(mutex_) { return QueueNotFullCondition(); },
|
||||
timeout)) {
|
||||
return false;
|
||||
}
|
||||
deque_.push_back(std::move(t));
|
||||
return true;
|
||||
}
|
||||
|
||||
// Pops the next value from the queue. Blocks until a value is available.
|
||||
T Pop() {
|
||||
MutexLocker lock(&mutex_);
|
||||
lock.Await([this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); });
|
||||
|
||||
T t = std::move(deque_.front());
|
||||
deque_.pop_front();
|
||||
return t;
|
||||
}
|
||||
|
||||
// Like Pop, but can timeout. Returns nullptr in this case.
|
||||
T PopWithTimeout(const common::Duration timeout) {
|
||||
MutexLocker lock(&mutex_);
|
||||
if (!lock.AwaitWithTimeout(
|
||||
[this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); },
|
||||
timeout)) {
|
||||
return nullptr;
|
||||
}
|
||||
T t = std::move(deque_.front());
|
||||
deque_.pop_front();
|
||||
return t;
|
||||
}
|
||||
|
||||
// Returns the next value in the queue or nullptr if the queue is empty.
|
||||
// Maintains ownership. This assumes a member function get() that returns
|
||||
// a pointer to the given type R.
|
||||
template <typename R>
|
||||
const R* Peek() {
|
||||
MutexLocker lock(&mutex_);
|
||||
if (deque_.empty()) {
|
||||
return nullptr;
|
||||
}
|
||||
return deque_.front().get();
|
||||
}
|
||||
|
||||
// Returns the number of items currently in the queue.
|
||||
size_t Size() {
|
||||
MutexLocker lock(&mutex_);
|
||||
return deque_.size();
|
||||
}
|
||||
|
||||
private:
|
||||
// Returns true iff the queue is not empty.
|
||||
bool QueueNotEmptyCondition() REQUIRES(mutex_) { return !deque_.empty(); }
|
||||
|
||||
// Returns true iff the queue is not full.
|
||||
bool QueueNotFullCondition() REQUIRES(mutex_) {
|
||||
return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
|
||||
}
|
||||
|
||||
Mutex mutex_;
|
||||
const size_t queue_size_ GUARDED_BY(mutex_);
|
||||
std::deque<T> deque_ GUARDED_BY(mutex_);
|
||||
};
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/blocking_queue.h"
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
namespace {
|
||||
|
||||
TEST(BlockingQueueTest, testPushPeekPop) {
|
||||
BlockingQueue<std::unique_ptr<int>> blocking_queue;
|
||||
blocking_queue.Push(common::make_unique<int>(42));
|
||||
ASSERT_EQ(1, blocking_queue.Size());
|
||||
blocking_queue.Push(common::make_unique<int>(24));
|
||||
ASSERT_EQ(2, blocking_queue.Size());
|
||||
EXPECT_EQ(42, *blocking_queue.Peek<int>());
|
||||
ASSERT_EQ(2, blocking_queue.Size());
|
||||
EXPECT_EQ(42, *blocking_queue.Pop());
|
||||
ASSERT_EQ(1, blocking_queue.Size());
|
||||
EXPECT_EQ(24, *blocking_queue.Pop());
|
||||
ASSERT_EQ(0, blocking_queue.Size());
|
||||
EXPECT_EQ(nullptr, blocking_queue.Peek<int>());
|
||||
ASSERT_EQ(0, blocking_queue.Size());
|
||||
}
|
||||
|
||||
TEST(BlockingQueueTest, testPushPopSharedPtr) {
|
||||
BlockingQueue<std::shared_ptr<int>> blocking_queue;
|
||||
blocking_queue.Push(std::make_shared<int>(42));
|
||||
blocking_queue.Push(std::make_shared<int>(24));
|
||||
EXPECT_EQ(42, *blocking_queue.Pop());
|
||||
EXPECT_EQ(24, *blocking_queue.Pop());
|
||||
}
|
||||
|
||||
TEST(BlockingQueueTest, testPopWithTimeout) {
|
||||
BlockingQueue<std::unique_ptr<int>> blocking_queue;
|
||||
EXPECT_EQ(nullptr,
|
||||
blocking_queue.PopWithTimeout(common::FromMilliseconds(150)));
|
||||
}
|
||||
|
||||
TEST(BlockingQueueTest, testPushWithTimeout) {
|
||||
BlockingQueue<std::unique_ptr<int>> blocking_queue(1);
|
||||
EXPECT_EQ(true,
|
||||
blocking_queue.PushWithTimeout(common::make_unique<int>(42),
|
||||
common::FromMilliseconds(150)));
|
||||
EXPECT_EQ(false,
|
||||
blocking_queue.PushWithTimeout(common::make_unique<int>(15),
|
||||
common::FromMilliseconds(150)));
|
||||
EXPECT_EQ(42, *blocking_queue.Pop());
|
||||
EXPECT_EQ(0, blocking_queue.Size());
|
||||
}
|
||||
|
||||
TEST(BlockingQueueTest, testPushWithTimeoutInfinteQueue) {
|
||||
BlockingQueue<std::unique_ptr<int>> blocking_queue;
|
||||
EXPECT_EQ(true,
|
||||
blocking_queue.PushWithTimeout(common::make_unique<int>(42),
|
||||
common::FromMilliseconds(150)));
|
||||
EXPECT_EQ(true,
|
||||
blocking_queue.PushWithTimeout(common::make_unique<int>(45),
|
||||
common::FromMilliseconds(150)));
|
||||
EXPECT_EQ(42, *blocking_queue.Pop());
|
||||
EXPECT_EQ(45, *blocking_queue.Pop());
|
||||
EXPECT_EQ(0, blocking_queue.Size());
|
||||
}
|
||||
|
||||
TEST(BlockingQueueTest, testBlockingPop) {
|
||||
BlockingQueue<std::unique_ptr<int>> blocking_queue;
|
||||
ASSERT_EQ(0, blocking_queue.Size());
|
||||
|
||||
int pop = 0;
|
||||
|
||||
std::thread thread([&blocking_queue, &pop] { pop = *blocking_queue.Pop(); });
|
||||
|
||||
std::this_thread::sleep_for(common::FromMilliseconds(100));
|
||||
blocking_queue.Push(common::make_unique<int>(42));
|
||||
thread.join();
|
||||
ASSERT_EQ(0, blocking_queue.Size());
|
||||
EXPECT_EQ(42, pop);
|
||||
}
|
||||
|
||||
TEST(BlockingQueueTest, testBlockingPopWithTimeout) {
|
||||
BlockingQueue<std::unique_ptr<int>> blocking_queue;
|
||||
ASSERT_EQ(0, blocking_queue.Size());
|
||||
|
||||
int pop = 0;
|
||||
|
||||
std::thread thread([&blocking_queue, &pop] {
|
||||
pop = *blocking_queue.PopWithTimeout(common::FromMilliseconds(2500));
|
||||
});
|
||||
|
||||
std::this_thread::sleep_for(common::FromMilliseconds(100));
|
||||
blocking_queue.Push(common::make_unique<int>(42));
|
||||
thread.join();
|
||||
ASSERT_EQ(0, blocking_queue.Size());
|
||||
EXPECT_EQ(42, pop);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -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
|
||||
|
|
@ -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_
|
||||
|
|
@ -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_
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/configuration_file_resolver.h"
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <streambuf>
|
||||
|
||||
#include "cartographer/common/config.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
ConfigurationFileResolver::ConfigurationFileResolver(
|
||||
const std::vector<string>& configuration_files_directories)
|
||||
: configuration_files_directories_(configuration_files_directories) {
|
||||
configuration_files_directories_.push_back(kConfigurationFilesDirectory);
|
||||
}
|
||||
|
||||
string ConfigurationFileResolver::GetFullPathOrDie(const string& basename) {
|
||||
for (const auto& path : configuration_files_directories_) {
|
||||
const string filename = path + "/" + basename;
|
||||
std::ifstream stream(filename.c_str());
|
||||
if (stream.good()) {
|
||||
return filename;
|
||||
}
|
||||
}
|
||||
LOG(FATAL) << "File " << basename << " was not found.";
|
||||
}
|
||||
|
||||
string ConfigurationFileResolver::GetFileContentOrDie(const string& basename) {
|
||||
const string filename = GetFullPathOrDie(basename);
|
||||
std::ifstream stream(filename.c_str());
|
||||
return string((std::istreambuf_iterator<char>(stream)),
|
||||
std::istreambuf_iterator<char>());
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_
|
||||
#define CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/port.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// A 'FileResolver' for the 'LuaParameterDictionary' that reads files from disk.
|
||||
// It searches the 'configuration_files_directories' in order to find the
|
||||
// requested filename. The last place searched is always the
|
||||
// 'configuration_files/' directory installed with Cartographer. It contains
|
||||
// reasonable configuration for the various Cartographer components which
|
||||
// provide a good starting ground for new platforms.
|
||||
class ConfigurationFileResolver : public FileResolver {
|
||||
public:
|
||||
explicit ConfigurationFileResolver(
|
||||
const std::vector<string>& configuration_files_directories);
|
||||
|
||||
string GetFullPathOrDie(const string& basename) override;
|
||||
string GetFileContentOrDie(const string& basename) override;
|
||||
|
||||
private:
|
||||
std::vector<string> configuration_files_directories_;
|
||||
};
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/fixed_ratio_sampler.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
FixedRatioSampler::FixedRatioSampler(const double ratio) : ratio_(ratio) {}
|
||||
|
||||
FixedRatioSampler::~FixedRatioSampler() {}
|
||||
|
||||
bool FixedRatioSampler::Pulse() {
|
||||
++num_pulses_;
|
||||
if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
|
||||
++num_samples_;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
string FixedRatioSampler::DebugString() {
|
||||
return std::to_string(num_samples_) + " (" +
|
||||
std::to_string(100. * num_samples_ / num_pulses_) + "%)";
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_
|
||||
#define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// Signals when a sample should be taken from a stream of data to select a
|
||||
// uniformly distributed fraction of the data.
|
||||
class FixedRatioSampler {
|
||||
public:
|
||||
explicit FixedRatioSampler(double ratio);
|
||||
~FixedRatioSampler();
|
||||
|
||||
FixedRatioSampler(const FixedRatioSampler&) = delete;
|
||||
FixedRatioSampler& operator=(const FixedRatioSampler&) = delete;
|
||||
|
||||
// Returns true if this pulse should result in an sample.
|
||||
bool Pulse();
|
||||
|
||||
// Returns a debug string describing the current ratio of samples to pulses.
|
||||
string DebugString();
|
||||
|
||||
private:
|
||||
// Sampling occurs if the proportion of samples to pulses drops below this
|
||||
// number.
|
||||
const double ratio_;
|
||||
|
||||
int64 num_pulses_ = 0;
|
||||
int64 num_samples_ = 0;
|
||||
};
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/histogram.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
#include <string>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
namespace {
|
||||
|
||||
string PaddedTo(string input, int new_length) {
|
||||
CHECK_GE(new_length, input.size());
|
||||
input.insert(input.begin(), new_length - input.size(), ' ');
|
||||
return input;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void BucketHistogram::Hit(const string& bucket) { ++buckets_[bucket]; }
|
||||
|
||||
string BucketHistogram::ToString() const {
|
||||
int64 sum = 0;
|
||||
size_t max_bucket_name_length = 0;
|
||||
for (const auto& pair : buckets_) {
|
||||
sum += pair.second;
|
||||
max_bucket_name_length =
|
||||
std::max(pair.first.size(), max_bucket_name_length);
|
||||
}
|
||||
|
||||
string result;
|
||||
for (const auto& pair : buckets_) {
|
||||
const float percent = 100.f * pair.second / std::max<int64>(1, sum);
|
||||
result += PaddedTo(pair.first, max_bucket_name_length) + ": " +
|
||||
PaddedTo(std::to_string(pair.second), 7) + " (" +
|
||||
std::to_string(percent) + " %)\n";
|
||||
}
|
||||
result += "Total: " + std::to_string(sum);
|
||||
return result;
|
||||
}
|
||||
|
||||
void Histogram::Add(const float value) { values_.push_back(value); }
|
||||
|
||||
string Histogram::ToString(const int buckets) const {
|
||||
CHECK_GE(buckets, 1);
|
||||
if (values_.empty()) {
|
||||
return "Count: 0";
|
||||
}
|
||||
const float min = *std::min_element(values_.begin(), values_.end());
|
||||
const float max = *std::max_element(values_.begin(), values_.end());
|
||||
const float mean =
|
||||
std::accumulate(values_.begin(), values_.end(), 0.f) / values_.size();
|
||||
string result = "Count: " + std::to_string(values_.size()) + " Min: " +
|
||||
std::to_string(min) + " Max: " + std::to_string(max) +
|
||||
" Mean: " + std::to_string(mean);
|
||||
if (min == max) {
|
||||
return result;
|
||||
}
|
||||
CHECK_LT(min, max);
|
||||
float lower_bound = min;
|
||||
int total_count = 0;
|
||||
for (int i = 0; i != buckets; ++i) {
|
||||
const float upper_bound =
|
||||
(i + 1 == buckets)
|
||||
? max
|
||||
: (max * (i + 1) / buckets + min * (buckets - i - 1) / buckets);
|
||||
int count = 0;
|
||||
for (const float value : values_) {
|
||||
if (lower_bound <= value &&
|
||||
(i + 1 == buckets ? value <= upper_bound : value < upper_bound)) {
|
||||
++count;
|
||||
}
|
||||
}
|
||||
total_count += count;
|
||||
result += "\n[" + std::to_string(lower_bound) + ", " +
|
||||
std::to_string(upper_bound) + ((i + 1 == buckets) ? "]" : ")");
|
||||
constexpr int kMaxBarChars = 20;
|
||||
const int bar =
|
||||
(count * kMaxBarChars + values_.size() / 2) / values_.size();
|
||||
result += "\t";
|
||||
for (int i = 0; i != kMaxBarChars; ++i) {
|
||||
result += (i < (kMaxBarChars - bar)) ? " " : "#";
|
||||
}
|
||||
result += "\tCount: " + std::to_string(count) + " (" +
|
||||
std::to_string(count * 1e2f / values_.size()) + "%)";
|
||||
result += "\tTotal: " + std::to_string(total_count) + " (" +
|
||||
std::to_string(total_count * 1e2f / values_.size()) + "%)";
|
||||
lower_bound = upper_bound;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_HISTOGRAM_H_
|
||||
#define CARTOGRAPHER_COMMON_HISTOGRAM_H_
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
class BucketHistogram {
|
||||
public:
|
||||
void Hit(const string& bucket);
|
||||
string ToString() const;
|
||||
|
||||
private:
|
||||
std::map<string, int64> buckets_;
|
||||
};
|
||||
|
||||
class Histogram {
|
||||
public:
|
||||
void Add(float value);
|
||||
string ToString(int buckets) const;
|
||||
|
||||
private:
|
||||
std::vector<float> values_;
|
||||
};
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_HISTOGRAM_H_
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_LUA_H_
|
||||
#define CARTOGRAPHER_COMMON_LUA_H_
|
||||
|
||||
#include <lua.hpp>
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_LUA_H_
|
||||
|
|
@ -0,0 +1,498 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
// When a LuaParameterDictionary is constructed, a new Lua state (i.e. an
|
||||
// independent Lua interpreter) is fired up to evaluate the Lua code. The code
|
||||
// is expected to return a Lua table that contains key/value pairs that are the
|
||||
// key/value pairs of our parameter dictionary.
|
||||
//
|
||||
// We keep the Lua interpreter around and the table on the stack and reference
|
||||
// it in the Get* methods instead of moving its contents from Lua into a C++ map
|
||||
// since we can only know in the Get* methods what data type to expect in the
|
||||
// table.
|
||||
//
|
||||
// Some of the methods below documentation the current stack with the following
|
||||
// notation: S: <bottom> ... <top>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
namespace {
|
||||
|
||||
// Replace the string at the top of the stack through a quoted version that Lua
|
||||
// can read back.
|
||||
void QuoteStringOnStack(lua_State* L) {
|
||||
CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value.";
|
||||
int current_index = lua_gettop(L);
|
||||
|
||||
// S: ... string
|
||||
lua_pushglobaltable(L); // S: ... string globals
|
||||
lua_getfield(L, -1, "string"); // S: ... string globals <string module>
|
||||
lua_getfield(L, -1,
|
||||
"format"); // S: ... string globals <string module> format
|
||||
lua_pushstring(L, "%q"); // S: ... string globals <string module> format "%q"
|
||||
lua_pushvalue(L, current_index); // S: ... string globals <string module>
|
||||
// format "%q" string
|
||||
|
||||
lua_call(L, 2, 1); // S: ... string globals <string module> quoted
|
||||
lua_replace(L, current_index); // S: ... quoted globals <string module>
|
||||
|
||||
lua_pop(L, 2); // S: ... quoted
|
||||
}
|
||||
|
||||
// Sets the given 'dictionary' as the value of the "this" key in Lua's registry
|
||||
// table.
|
||||
void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) {
|
||||
lua_pushstring(L, "this");
|
||||
lua_pushlightuserdata(L, dictionary);
|
||||
lua_settable(L, LUA_REGISTRYINDEX);
|
||||
}
|
||||
|
||||
// Gets the 'dictionary' from the "this" key in Lua's registry table.
|
||||
LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) {
|
||||
lua_pushstring(L, "this");
|
||||
lua_gettable(L, LUA_REGISTRYINDEX);
|
||||
void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1);
|
||||
lua_pop(L, 1);
|
||||
CHECK(return_value != nullptr);
|
||||
return reinterpret_cast<LuaParameterDictionary*>(return_value);
|
||||
}
|
||||
|
||||
// CHECK()s if a Lua method returned an error.
|
||||
void CheckForLuaErrors(lua_State* L, int status) {
|
||||
CHECK_EQ(status, 0) << lua_tostring(L, -1);
|
||||
}
|
||||
|
||||
// Returns 'a' if 'condition' is true, else 'b'.
|
||||
int LuaChoose(lua_State* L) {
|
||||
CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b).";
|
||||
CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value.";
|
||||
|
||||
const bool condition = lua_toboolean(L, 1);
|
||||
if (condition) {
|
||||
lua_pushvalue(L, 2);
|
||||
} else {
|
||||
lua_pushvalue(L, 3);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Pushes a value to the Lua stack.
|
||||
void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); }
|
||||
void PushValue(lua_State* L, const string& key) {
|
||||
lua_pushstring(L, key.c_str());
|
||||
}
|
||||
|
||||
// Reads the value with the given 'key' from the Lua dictionary and pushes it to
|
||||
// the top of the stack.
|
||||
template <typename T>
|
||||
void GetValueFromLuaTable(lua_State* L, const T& key) {
|
||||
PushValue(L, key);
|
||||
lua_rawget(L, -2);
|
||||
}
|
||||
|
||||
// CHECK() that the topmost parameter on the Lua stack is a table.
|
||||
void CheckTableIsAtTopOfStack(lua_State* L) {
|
||||
CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
|
||||
}
|
||||
|
||||
// Returns true if 'key' is in the table at the top of the Lua stack.
|
||||
template <typename T>
|
||||
bool HasKeyOfType(lua_State* L, const T& key) {
|
||||
CheckTableIsAtTopOfStack(L);
|
||||
PushValue(L, key);
|
||||
lua_rawget(L, -2);
|
||||
const bool key_not_found = lua_isnil(L, -1);
|
||||
lua_pop(L, 1); // Pop the item again.
|
||||
return !key_not_found;
|
||||
}
|
||||
|
||||
// Iterates over the integer keys of the table at the top of the stack of 'L•
|
||||
// and pushes the values one by one. 'pop_value' is expected to pop a value and
|
||||
// put them into a C++ container.
|
||||
void GetArrayValues(lua_State* L, const std::function<void()>& pop_value) {
|
||||
int idx = 1;
|
||||
while (true) {
|
||||
GetValueFromLuaTable(L, idx);
|
||||
if (lua_isnil(L, -1)) {
|
||||
lua_pop(L, 1);
|
||||
break;
|
||||
}
|
||||
pop_value();
|
||||
++idx;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary>
|
||||
LuaParameterDictionary::NonReferenceCounted(
|
||||
const string& code, std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_function) {
|
||||
return std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
|
||||
code, ReferenceCount::NO, std::move(file_resolver),
|
||||
state_extension_function));
|
||||
}
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::Partial(
|
||||
const string& code, const string& key,
|
||||
std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_function) {
|
||||
auto parameter_dictionary =
|
||||
std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
|
||||
code, std::move(file_resolver), state_extension_function));
|
||||
// This replaces the table at the top of the stack with the table at 'key'.
|
||||
auto& L = parameter_dictionary->L_;
|
||||
|
||||
const string lua_code = "local table=...; return table." + key;
|
||||
CheckForLuaErrors(L, luaL_loadstring(L, lua_code.c_str()));
|
||||
lua_pushvalue(L, -2); // S: table, function, table
|
||||
lua_remove(L, -3); // S: function, table
|
||||
CheckForLuaErrors(L, lua_pcall(L, 1, 1, 0));
|
||||
CheckTableIsAtTopOfStack(L);
|
||||
return parameter_dictionary;
|
||||
}
|
||||
|
||||
LuaParameterDictionary::LuaParameterDictionary(
|
||||
const string& code, std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_function)
|
||||
: LuaParameterDictionary(code, ReferenceCount::YES,
|
||||
std::move(file_resolver),
|
||||
state_extension_function) {}
|
||||
|
||||
LuaParameterDictionary::LuaParameterDictionary(
|
||||
const string& code, ReferenceCount reference_count,
|
||||
std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_function)
|
||||
: L_(luaL_newstate()),
|
||||
index_into_reference_table_(-1),
|
||||
file_resolver_(std::move(file_resolver)),
|
||||
reference_count_(reference_count) {
|
||||
CHECK_NOTNULL(L_);
|
||||
SetDictionaryInRegistry(L_, this);
|
||||
|
||||
luaL_openlibs(L_);
|
||||
|
||||
lua_register(L_, "choose", LuaChoose);
|
||||
lua_register(L_, "include", LuaInclude);
|
||||
lua_register(L_, "read", LuaRead);
|
||||
if (state_extension_function) {
|
||||
state_extension_function(L_);
|
||||
}
|
||||
|
||||
CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str()));
|
||||
CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0));
|
||||
CheckTableIsAtTopOfStack(L_);
|
||||
}
|
||||
|
||||
LuaParameterDictionary::LuaParameterDictionary(
|
||||
lua_State* const L, ReferenceCount reference_count,
|
||||
std::shared_ptr<FileResolver> file_resolver)
|
||||
: L_(lua_newthread(L)),
|
||||
file_resolver_(std::move(file_resolver)),
|
||||
reference_count_(reference_count) {
|
||||
CHECK_NOTNULL(L_);
|
||||
|
||||
// Make sure this is never garbage collected.
|
||||
CHECK(lua_isthread(L, -1));
|
||||
index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX);
|
||||
|
||||
CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
|
||||
lua_xmove(L, L_, 1); // Moves the table and the coroutine over.
|
||||
CheckTableIsAtTopOfStack(L_);
|
||||
}
|
||||
|
||||
LuaParameterDictionary::~LuaParameterDictionary() {
|
||||
if (reference_count_ == ReferenceCount::YES) {
|
||||
CheckAllKeysWereUsedExactlyOnceAndReset();
|
||||
}
|
||||
if (index_into_reference_table_ > 0) {
|
||||
luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_);
|
||||
} else {
|
||||
lua_close(L_);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<string> LuaParameterDictionary::GetKeys() const {
|
||||
CheckTableIsAtTopOfStack(L_);
|
||||
std::vector<string> keys;
|
||||
|
||||
lua_pushnil(L_); // Push the first key
|
||||
while (lua_next(L_, -2) != 0) {
|
||||
lua_pop(L_, 1); // Pop value, keep key.
|
||||
if (!lua_isnumber(L_, -1)) {
|
||||
keys.emplace_back(lua_tostring(L_, -1));
|
||||
}
|
||||
}
|
||||
return keys;
|
||||
}
|
||||
|
||||
bool LuaParameterDictionary::HasKey(const string& key) const {
|
||||
return HasKeyOfType(L_, key);
|
||||
}
|
||||
|
||||
string LuaParameterDictionary::GetString(const string& key) {
|
||||
CheckHasKeyAndReference(key);
|
||||
GetValueFromLuaTable(L_, key);
|
||||
return PopString(Quoted::NO);
|
||||
}
|
||||
|
||||
string LuaParameterDictionary::PopString(Quoted quoted) const {
|
||||
CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value.";
|
||||
if (quoted == Quoted::YES) {
|
||||
QuoteStringOnStack(L_);
|
||||
}
|
||||
|
||||
const string value = lua_tostring(L_, -1);
|
||||
lua_pop(L_, 1);
|
||||
return value;
|
||||
}
|
||||
|
||||
double LuaParameterDictionary::GetDouble(const string& key) {
|
||||
CheckHasKeyAndReference(key);
|
||||
GetValueFromLuaTable(L_, key);
|
||||
return PopDouble();
|
||||
}
|
||||
|
||||
double LuaParameterDictionary::PopDouble() const {
|
||||
CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
|
||||
const double value = lua_tonumber(L_, -1);
|
||||
lua_pop(L_, 1);
|
||||
return value;
|
||||
}
|
||||
|
||||
int LuaParameterDictionary::GetInt(const string& key) {
|
||||
CheckHasKeyAndReference(key);
|
||||
GetValueFromLuaTable(L_, key);
|
||||
return PopInt();
|
||||
}
|
||||
|
||||
int LuaParameterDictionary::PopInt() const {
|
||||
CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
|
||||
const int value = lua_tointeger(L_, -1);
|
||||
lua_pop(L_, 1);
|
||||
return value;
|
||||
}
|
||||
|
||||
bool LuaParameterDictionary::GetBool(const string& key) {
|
||||
CheckHasKeyAndReference(key);
|
||||
GetValueFromLuaTable(L_, key);
|
||||
return PopBool();
|
||||
}
|
||||
|
||||
bool LuaParameterDictionary::PopBool() const {
|
||||
CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value.";
|
||||
const bool value = lua_toboolean(L_, -1);
|
||||
lua_pop(L_, 1);
|
||||
return value;
|
||||
}
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
|
||||
const string& key) {
|
||||
CheckHasKeyAndReference(key);
|
||||
GetValueFromLuaTable(L_, key);
|
||||
return PopDictionary(reference_count_);
|
||||
}
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::PopDictionary(
|
||||
ReferenceCount reference_count) const {
|
||||
CheckTableIsAtTopOfStack(L_);
|
||||
std::unique_ptr<LuaParameterDictionary> value(
|
||||
new LuaParameterDictionary(L_, reference_count, file_resolver_));
|
||||
// The constructor lua_xmove()s the value, no need to pop it.
|
||||
CheckTableIsAtTopOfStack(L_);
|
||||
return value;
|
||||
}
|
||||
|
||||
string LuaParameterDictionary::DoToString(const string& indent) const {
|
||||
string result = "{";
|
||||
bool dictionary_is_empty = true;
|
||||
|
||||
const auto top_of_stack_to_string = [this, indent,
|
||||
&dictionary_is_empty]() -> string {
|
||||
dictionary_is_empty = false;
|
||||
|
||||
const int value_type = lua_type(L_, -1);
|
||||
switch (value_type) {
|
||||
case LUA_TBOOLEAN:
|
||||
return PopBool() ? "true" : "false";
|
||||
break;
|
||||
case LUA_TSTRING:
|
||||
return PopString(Quoted::YES);
|
||||
break;
|
||||
case LUA_TNUMBER: {
|
||||
const double value = PopDouble();
|
||||
if (std::isinf(value)) {
|
||||
return value < 0 ? "-math.huge" : "math.huge";
|
||||
} else {
|
||||
return std::to_string(value);
|
||||
}
|
||||
} break;
|
||||
case LUA_TTABLE: {
|
||||
std::unique_ptr<LuaParameterDictionary> subdict(
|
||||
PopDictionary(ReferenceCount::NO));
|
||||
return subdict->DoToString(indent + " ");
|
||||
} break;
|
||||
default:
|
||||
LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type);
|
||||
}
|
||||
};
|
||||
|
||||
// Integer (array) keys.
|
||||
for (int i = 1; i; ++i) {
|
||||
GetValueFromLuaTable(L_, i);
|
||||
if (lua_isnil(L_, -1)) {
|
||||
lua_pop(L_, 1);
|
||||
break;
|
||||
}
|
||||
result.append("\n");
|
||||
result.append(indent);
|
||||
result.append(" ");
|
||||
result.append(top_of_stack_to_string());
|
||||
result.append(",");
|
||||
}
|
||||
|
||||
// String keys.
|
||||
std::vector<string> keys = GetKeys();
|
||||
if (!keys.empty()) {
|
||||
std::sort(keys.begin(), keys.end());
|
||||
for (const string& key : keys) {
|
||||
GetValueFromLuaTable(L_, key);
|
||||
result.append("\n");
|
||||
result.append(indent);
|
||||
result.append(" ");
|
||||
result.append(key);
|
||||
result.append(" = ");
|
||||
result.append(top_of_stack_to_string());
|
||||
result.append(",");
|
||||
}
|
||||
}
|
||||
result.append("\n");
|
||||
result.append(indent);
|
||||
result.append("}");
|
||||
|
||||
if (dictionary_is_empty) {
|
||||
return "{}";
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
string LuaParameterDictionary::ToString() const { return DoToString(""); }
|
||||
|
||||
std::vector<double> LuaParameterDictionary::GetArrayValuesAsDoubles() {
|
||||
std::vector<double> values;
|
||||
GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); });
|
||||
return values;
|
||||
}
|
||||
|
||||
std::vector<std::unique_ptr<LuaParameterDictionary>>
|
||||
LuaParameterDictionary::GetArrayValuesAsDictionaries() {
|
||||
std::vector<std::unique_ptr<LuaParameterDictionary>> values;
|
||||
GetArrayValues(L_, [&values, this] {
|
||||
values.push_back(PopDictionary(reference_count_));
|
||||
});
|
||||
return values;
|
||||
}
|
||||
|
||||
std::vector<string> LuaParameterDictionary::GetArrayValuesAsStrings() {
|
||||
std::vector<string> values;
|
||||
GetArrayValues(L_,
|
||||
[&values, this] { values.push_back(PopString(Quoted::NO)); });
|
||||
return values;
|
||||
}
|
||||
|
||||
void LuaParameterDictionary::CheckHasKey(const string& key) const {
|
||||
CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n"
|
||||
<< ToString();
|
||||
}
|
||||
|
||||
void LuaParameterDictionary::CheckHasKeyAndReference(const string& key) {
|
||||
CheckHasKey(key);
|
||||
reference_counts_[key]++;
|
||||
}
|
||||
|
||||
void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() {
|
||||
for (const auto& key : GetKeys()) {
|
||||
CHECK_EQ(1, reference_counts_.count(key))
|
||||
<< "Key '" << key << "' was used the wrong number of times.";
|
||||
CHECK_EQ(1, reference_counts_.at(key))
|
||||
<< "Key '" << key << "' was used the wrong number of times.";
|
||||
}
|
||||
reference_counts_.clear();
|
||||
}
|
||||
|
||||
int LuaParameterDictionary::GetNonNegativeInt(const string& key) {
|
||||
const int temp = GetInt(key); // Will increase reference count.
|
||||
CHECK_GE(temp, 0) << temp << " is negative.";
|
||||
return temp;
|
||||
}
|
||||
|
||||
// Lua function to run a script in the current Lua context. Takes the filename
|
||||
// as its only argument.
|
||||
int LuaParameterDictionary::LuaInclude(lua_State* L) {
|
||||
CHECK_EQ(lua_gettop(L), 1);
|
||||
CHECK(lua_isstring(L, -1)) << "include takes a filename.";
|
||||
|
||||
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
|
||||
const string basename = lua_tostring(L, -1);
|
||||
const string filename =
|
||||
parameter_dictionary->file_resolver_->GetFullPathOrDie(basename);
|
||||
if (std::find(parameter_dictionary->included_files_.begin(),
|
||||
parameter_dictionary->included_files_.end(),
|
||||
filename) != parameter_dictionary->included_files_.end()) {
|
||||
string error_msg = "Tried to include " + filename +
|
||||
" twice. Already included files in order of inclusion: ";
|
||||
for (const string& filename : parameter_dictionary->included_files_) {
|
||||
error_msg.append(filename);
|
||||
error_msg.append("\n");
|
||||
}
|
||||
LOG(FATAL) << error_msg;
|
||||
}
|
||||
parameter_dictionary->included_files_.push_back(filename);
|
||||
lua_pop(L, 1);
|
||||
CHECK_EQ(lua_gettop(L), 0);
|
||||
|
||||
const string content =
|
||||
parameter_dictionary->file_resolver_->GetFileContentOrDie(basename);
|
||||
CheckForLuaErrors(
|
||||
L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
|
||||
CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0));
|
||||
|
||||
return lua_gettop(L);
|
||||
}
|
||||
|
||||
// Lua function to read a file into a string.
|
||||
int LuaParameterDictionary::LuaRead(lua_State* L) {
|
||||
CHECK_EQ(lua_gettop(L), 1);
|
||||
CHECK(lua_isstring(L, -1)) << "read takes a filename.";
|
||||
|
||||
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
|
||||
const string file_content =
|
||||
parameter_dictionary->file_resolver_->GetFileContentOrDie(
|
||||
lua_tostring(L, -1));
|
||||
lua_pushstring(L, file_content.c_str());
|
||||
return 1;
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,163 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_
|
||||
#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/lua.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// Resolves file paths and file content for the Lua 'read' and 'include'
|
||||
// functions. Use this to configure where those functions load other files from.
|
||||
class FileResolver {
|
||||
public:
|
||||
virtual ~FileResolver() {}
|
||||
virtual string GetFullPathOrDie(const string& basename) = 0;
|
||||
virtual string GetFileContentOrDie(const string& basename) = 0;
|
||||
};
|
||||
|
||||
// A function that adds new Lua functions to a state. Used to extend the Lua
|
||||
// configuration in custom contexts.
|
||||
using StateExtensionFunction = std::function<void(lua_State*)>;
|
||||
|
||||
// A parameter dictionary that gets loaded from Lua code.
|
||||
class LuaParameterDictionary {
|
||||
public:
|
||||
// Constructs the dictionary from a Lua Table specification.
|
||||
LuaParameterDictionary(const string& code,
|
||||
std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_functon);
|
||||
|
||||
LuaParameterDictionary(const LuaParameterDictionary&) = delete;
|
||||
LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete;
|
||||
|
||||
// Constructs a LuaParameterDictionary without reference counting.
|
||||
static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(
|
||||
const string& code, std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_functon);
|
||||
|
||||
// Constructs a partial LuaParameterDictionary by extracting the dictionary
|
||||
// with 'key' from 'code' where 'key' refers to an arbitrarily deep dictionary
|
||||
// (e.g. "a.b.c").
|
||||
static std::unique_ptr<LuaParameterDictionary> Partial(
|
||||
const string& code, const string& key,
|
||||
std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_functon);
|
||||
|
||||
~LuaParameterDictionary();
|
||||
|
||||
// Returns all available keys.
|
||||
std::vector<string> GetKeys() const;
|
||||
|
||||
// Returns true if the key is in this dictionary.
|
||||
bool HasKey(const string& key) const;
|
||||
|
||||
// These methods CHECK() that the 'key' exists.
|
||||
string GetString(const string& key);
|
||||
double GetDouble(const string& key);
|
||||
int GetInt(const string& key);
|
||||
bool GetBool(const string& key);
|
||||
std::unique_ptr<LuaParameterDictionary> GetDictionary(const string& key);
|
||||
|
||||
// Gets an int from the dictionary and CHECK()s that it is non-negative.
|
||||
int GetNonNegativeInt(const string& key);
|
||||
|
||||
// Returns a string representation for this LuaParameterDictionary.
|
||||
string ToString() const;
|
||||
|
||||
// Returns the values of the keys '1', '2', '3' as the given types.
|
||||
std::vector<double> GetArrayValuesAsDoubles();
|
||||
std::vector<string> GetArrayValuesAsStrings();
|
||||
std::vector<std::unique_ptr<LuaParameterDictionary>>
|
||||
GetArrayValuesAsDictionaries();
|
||||
|
||||
private:
|
||||
enum class ReferenceCount { YES, NO };
|
||||
LuaParameterDictionary(const string& code, ReferenceCount reference_count,
|
||||
std::unique_ptr<FileResolver> file_resolver,
|
||||
StateExtensionFunction state_extension_function);
|
||||
|
||||
// For GetDictionary().
|
||||
LuaParameterDictionary(lua_State* L, ReferenceCount reference_count,
|
||||
std::shared_ptr<FileResolver> file_resolver);
|
||||
|
||||
// Function that recurses to keep track of indent for ToString().
|
||||
string DoToString(const string& indent) const;
|
||||
|
||||
// Pop the top of the stack and CHECKs that the type is correct.
|
||||
double PopDouble() const;
|
||||
int PopInt() const;
|
||||
bool PopBool() const;
|
||||
|
||||
// Pop the top of the stack and CHECKs that it is a string. The returned value
|
||||
// is either quoted to be suitable to be read back by a Lua interpretor or
|
||||
// not.
|
||||
enum class Quoted { YES, NO };
|
||||
string PopString(Quoted quoted) const;
|
||||
|
||||
// Creates a LuaParameterDictionary from the Lua table at the top of the
|
||||
// stack, either with or without reference counting.
|
||||
std::unique_ptr<LuaParameterDictionary> PopDictionary(
|
||||
ReferenceCount reference_count) const;
|
||||
|
||||
// CHECK() that 'key' is in the dictionary.
|
||||
void CheckHasKey(const string& key) const;
|
||||
|
||||
// CHECK() that 'key' is in this dictionary and reference it as being used.
|
||||
void CheckHasKeyAndReference(const string& key);
|
||||
|
||||
// If desired, this can be called in the destructor of a derived class. It
|
||||
// will CHECK() that all keys defined in the configuration have been used
|
||||
// exactly once and resets the reference counter.
|
||||
void CheckAllKeysWereUsedExactlyOnceAndReset();
|
||||
|
||||
// Reads a file into a Lua string.
|
||||
static int LuaRead(lua_State* L);
|
||||
|
||||
// Handles inclusion of other Lua files and prevents double inclusion.
|
||||
static int LuaInclude(lua_State* L);
|
||||
|
||||
lua_State* L_; // The name is by convention in the Lua World.
|
||||
int index_into_reference_table_;
|
||||
|
||||
// This is shared with all the sub dictionaries.
|
||||
const std::shared_ptr<FileResolver> file_resolver_;
|
||||
|
||||
// If true will check that all keys were used on destruction.
|
||||
const ReferenceCount reference_count_;
|
||||
|
||||
// This is modified with every call to Get* in order to verify that all
|
||||
// parameters are read exactly once.
|
||||
std::map<string, int> reference_counts_;
|
||||
|
||||
// List of all included files in order of inclusion. Used to prevent double
|
||||
// inclusion.
|
||||
std::vector<string> included_files_;
|
||||
};
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_
|
||||
|
|
@ -0,0 +1,245 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
namespace {
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary> MakeNonReferenceCounted(
|
||||
const string& code) {
|
||||
return LuaParameterDictionary::NonReferenceCounted(
|
||||
code, common::make_unique<DummyFileResolver>(),
|
||||
nullptr /* state_extension_function */);
|
||||
}
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary> MakePartial(const string& code,
|
||||
const string& key) {
|
||||
return LuaParameterDictionary::Partial(
|
||||
code, key, common::make_unique<DummyFileResolver>(),
|
||||
nullptr /* state_extension_function */);
|
||||
}
|
||||
|
||||
class LuaParameterDictionaryTest : public ::testing::Test {
|
||||
protected:
|
||||
void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) {
|
||||
for (const string& key : dict->GetKeys()) {
|
||||
dict->GetInt(key);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetInt) {
|
||||
auto dict = MakeDictionary("return { blah = 100 }");
|
||||
ASSERT_EQ(dict->GetInt("blah"), 100);
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetString) {
|
||||
auto dict = MakeDictionary("return { blah = 'is_a_string' }\n");
|
||||
ASSERT_EQ(dict->GetString("blah"), "is_a_string");
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetDouble) {
|
||||
auto dict = MakeDictionary("return { blah = 3.1415 }");
|
||||
ASSERT_DOUBLE_EQ(dict->GetDouble("blah"), 3.1415);
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetBoolTrue) {
|
||||
auto dict = MakeDictionary("return { blah = true }");
|
||||
ASSERT_TRUE(dict->GetBool("blah"));
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetBoolFalse) {
|
||||
auto dict = MakeDictionary("return { blah = false }");
|
||||
ASSERT_FALSE(dict->GetBool("blah"));
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetDictionary) {
|
||||
auto dict =
|
||||
MakeDictionary("return { blah = { blue = 100, red = 200 }, fasel = 10 }");
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary> sub_dict(dict->GetDictionary("blah"));
|
||||
std::vector<string> keys = sub_dict->GetKeys();
|
||||
ASSERT_EQ(keys.size(), 2);
|
||||
std::sort(keys.begin(), keys.end());
|
||||
ASSERT_EQ(keys[0], "blue");
|
||||
ASSERT_EQ(keys[1], "red");
|
||||
ASSERT_TRUE(sub_dict->HasKey("blue"));
|
||||
ASSERT_TRUE(sub_dict->HasKey("red"));
|
||||
ASSERT_EQ(sub_dict->GetInt("blue"), 100);
|
||||
ASSERT_EQ(sub_dict->GetInt("red"), 200);
|
||||
|
||||
ASSERT_EQ(dict->GetString("fasel"), "10");
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetKeys) {
|
||||
auto dict = MakeDictionary("return { blah = 100, blah1 = 200}");
|
||||
|
||||
std::vector<string> keys = dict->GetKeys();
|
||||
ASSERT_EQ(keys.size(), 2);
|
||||
std::sort(keys.begin(), keys.end());
|
||||
ASSERT_EQ(keys[0], "blah");
|
||||
ASSERT_EQ(keys[1], "blah1");
|
||||
|
||||
ReferenceAllKeysAsIntegers(dict.get());
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, NonExistingKey) {
|
||||
auto dict = MakeDictionary("return { blah = 100 }");
|
||||
ReferenceAllKeysAsIntegers(dict.get());
|
||||
ASSERT_DEATH(dict->GetInt("blah_fasel"), "Key.* not in dictionary.");
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, UintNegative) {
|
||||
auto dict = MakeDictionary("return { blah = -100}");
|
||||
ASSERT_DEATH(dict->GetNonNegativeInt("blah"), ".*-100 is negative.");
|
||||
ReferenceAllKeysAsIntegers(dict.get());
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, ToString) {
|
||||
auto dict = MakeDictionary(R"(return {
|
||||
ceta = { yolo = "hurray" },
|
||||
fasel = 1234.456786,
|
||||
fasel1 = -math.huge,
|
||||
fasel2 = math.huge,
|
||||
blubber = 123,
|
||||
blub = 'hello',
|
||||
alpha = true,
|
||||
alpha1 = false,
|
||||
})");
|
||||
|
||||
const string golden = R"({
|
||||
alpha = true,
|
||||
alpha1 = false,
|
||||
blub = "hello",
|
||||
blubber = 123.000000,
|
||||
ceta = {
|
||||
yolo = "hurray",
|
||||
},
|
||||
fasel = 1234.456786,
|
||||
fasel1 = -math.huge,
|
||||
fasel2 = math.huge,
|
||||
})";
|
||||
EXPECT_EQ(golden, dict->ToString());
|
||||
|
||||
auto dict1 = MakeDictionary("return " + dict->ToString());
|
||||
|
||||
EXPECT_EQ(dict1->GetBool("alpha"), true);
|
||||
EXPECT_EQ(dict1->GetBool("alpha1"), false);
|
||||
EXPECT_EQ(dict1->GetInt("blubber"), 123);
|
||||
EXPECT_EQ(dict1->GetString("blub"), "hello");
|
||||
EXPECT_EQ(dict1->GetDictionary("ceta")->GetString("yolo"), "hurray");
|
||||
EXPECT_NEAR(dict1->GetDouble("fasel"), 1234.456786, 1e-3);
|
||||
EXPECT_TRUE(std::isinf(-dict1->GetDouble("fasel1")));
|
||||
EXPECT_TRUE(std::isinf(dict1->GetDouble("fasel2")));
|
||||
|
||||
EXPECT_EQ(dict->GetBool("alpha"), true);
|
||||
EXPECT_EQ(dict->GetBool("alpha1"), false);
|
||||
EXPECT_EQ(dict->GetInt("blubber"), 123);
|
||||
EXPECT_EQ(dict->GetString("blub"), "hello");
|
||||
EXPECT_EQ(dict->GetDictionary("ceta")->GetString("yolo"), "hurray");
|
||||
EXPECT_NEAR(dict->GetDouble("fasel"), 1234.456786, 1e-3);
|
||||
EXPECT_TRUE(std::isinf(-dict->GetDouble("fasel1")));
|
||||
EXPECT_TRUE(std::isinf(dict->GetDouble("fasel2")));
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, ToStringForArrays) {
|
||||
auto dict = MakeNonReferenceCounted(
|
||||
R"(return {
|
||||
"blub", 3, 3.1,
|
||||
foo = "ups",
|
||||
})");
|
||||
|
||||
const string golden = R"({
|
||||
"blub",
|
||||
3.000000,
|
||||
3.100000,
|
||||
foo = "ups",
|
||||
})";
|
||||
EXPECT_EQ(golden, dict->ToString());
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) {
|
||||
auto dict = MakeDictionary("return { 'a', 'b', 'c' }");
|
||||
EXPECT_EQ(0, dict->GetKeys().size());
|
||||
const std::vector<string> values = dict->GetArrayValuesAsStrings();
|
||||
EXPECT_EQ(3, values.size());
|
||||
EXPECT_EQ("a", values[0]);
|
||||
EXPECT_EQ("b", values[1]);
|
||||
EXPECT_EQ("c", values[2]);
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDoubles) {
|
||||
auto dict = MakeDictionary("return { 1., 2., 3. }");
|
||||
EXPECT_EQ(0, dict->GetKeys().size());
|
||||
const std::vector<double> values = dict->GetArrayValuesAsDoubles();
|
||||
EXPECT_EQ(3, values.size());
|
||||
EXPECT_NEAR(1., values[0], 1e-7);
|
||||
EXPECT_NEAR(2., values[1], 1e-7);
|
||||
EXPECT_NEAR(3., values[2], 1e-7);
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDictionaries) {
|
||||
auto dict = MakeDictionary("return { { a = 1 }, { b = 3 } }");
|
||||
EXPECT_EQ(0, dict->GetKeys().size());
|
||||
const std::vector<std::unique_ptr<LuaParameterDictionary>> values =
|
||||
dict->GetArrayValuesAsDictionaries();
|
||||
EXPECT_EQ(2, values.size());
|
||||
EXPECT_EQ(1., values[0]->GetInt("a"));
|
||||
EXPECT_EQ(3., values[1]->GetInt("b"));
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, TestChooseTrue) {
|
||||
auto dict = MakeDictionary("return { a = choose(true, 1, 0) }");
|
||||
EXPECT_EQ(1, dict->GetInt("a"));
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, TestChoseFalse) {
|
||||
auto dict = MakeDictionary("return { a = choose(false, 1, 0) }");
|
||||
EXPECT_EQ(0, dict->GetInt("a"));
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, TestChooseInvalidArgument) {
|
||||
EXPECT_DEATH(MakeDictionary("return { a = choose('truish', 1, 0) }"),
|
||||
"condition is not a boolean value.");
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, Partial) {
|
||||
auto partial_dictionary =
|
||||
MakePartial("return { blah = { blue = { red = 200 } } }", "blah.blue");
|
||||
EXPECT_EQ(200, partial_dictionary->GetInt("red"));
|
||||
}
|
||||
|
||||
TEST_F(LuaParameterDictionaryTest, PartialIsReferenceCounted) {
|
||||
auto partial_dictionary =
|
||||
MakePartial("return { blah = { blue = { red = 200 } } }", "blah.blue");
|
||||
ASSERT_DEATH(partial_dictionary.reset(),
|
||||
".*Key 'red' was used the wrong number of times..*");
|
||||
partial_dictionary->GetInt("red");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
|
||||
#define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
class DummyFileResolver : public FileResolver {
|
||||
public:
|
||||
DummyFileResolver() {}
|
||||
|
||||
DummyFileResolver(const DummyFileResolver&) = delete;
|
||||
DummyFileResolver& operator=(const DummyFileResolver&) = delete;
|
||||
|
||||
~DummyFileResolver() override {}
|
||||
|
||||
string GetFileContentOrDie(const string& unused_basename) override {
|
||||
LOG(FATAL) << "Not implemented";
|
||||
}
|
||||
|
||||
string GetFullPathOrDie(const string& unused_basename) override {
|
||||
LOG(FATAL) << "Not implemented";
|
||||
}
|
||||
};
|
||||
|
||||
std::unique_ptr<LuaParameterDictionary> MakeDictionary(const string& code) {
|
||||
return common::make_unique<LuaParameterDictionary>(
|
||||
code, std::unique_ptr<DummyFileResolver>(new DummyFileResolver()),
|
||||
nullptr /* state_extension_function */);
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_
|
||||
#define CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// Implementation of c++14's std::make_unique, taken from
|
||||
// https://isocpp.org/files/papers/N3656.txt
|
||||
template <class T>
|
||||
struct _Unique_if {
|
||||
typedef std::unique_ptr<T> _Single_object;
|
||||
};
|
||||
|
||||
template <class T>
|
||||
struct _Unique_if<T[]> {
|
||||
typedef std::unique_ptr<T[]> _Unknown_bound;
|
||||
};
|
||||
|
||||
template <class T, size_t N>
|
||||
struct _Unique_if<T[N]> {
|
||||
typedef void _Known_bound;
|
||||
};
|
||||
|
||||
template <class T, class... Args>
|
||||
typename _Unique_if<T>::_Single_object make_unique(Args&&... args) {
|
||||
return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
|
||||
}
|
||||
|
||||
template <class T>
|
||||
typename _Unique_if<T>::_Unknown_bound make_unique(size_t n) {
|
||||
typedef typename std::remove_extent<T>::type U;
|
||||
return std::unique_ptr<T>(new U[n]());
|
||||
}
|
||||
|
||||
template <class T, class... Args>
|
||||
typename _Unique_if<T>::_Known_bound make_unique(Args&&...) = delete;
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_MATH_H_
|
||||
#define CARTOGRAPHER_COMMON_MATH_H_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "ceres/ceres.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// Clamps 'value' to be in the range ['min', 'max'].
|
||||
template <typename T>
|
||||
T Clamp(const T value, const T min, const T max) {
|
||||
if (value > max) {
|
||||
return max;
|
||||
}
|
||||
if (value < min) {
|
||||
return min;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
// Calculates 'base'^'exponent'.
|
||||
template <typename T>
|
||||
constexpr T Power(T base, int exponent) {
|
||||
return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
|
||||
}
|
||||
|
||||
// Calculates a^2.
|
||||
template <typename T>
|
||||
constexpr T Pow2(T a) {
|
||||
return Power(a, 2);
|
||||
}
|
||||
|
||||
// Calculates the real part of the square root of 'a'. This is helpful when
|
||||
// rounding errors generate a small negative argument. Otherwise std::sqrt
|
||||
// returns NaN if its argument is negative.
|
||||
template <typename T>
|
||||
constexpr T RealSqrt(T a) {
|
||||
return sqrt(std::max(T(0.), a));
|
||||
}
|
||||
|
||||
// Converts from degrees to radians.
|
||||
constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
|
||||
|
||||
// Converts form radians to degrees.
|
||||
constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
|
||||
|
||||
// Bring the 'difference' between two angles into [-pi; pi].
|
||||
template <typename T>
|
||||
T NormalizeAngleDifference(T difference) {
|
||||
while (difference > M_PI) {
|
||||
difference -= T(2. * M_PI);
|
||||
}
|
||||
while (difference < -M_PI) {
|
||||
difference += T(2. * M_PI);
|
||||
}
|
||||
return difference;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
|
||||
return ceres::atan2(vector.y(), vector.x());
|
||||
}
|
||||
|
||||
// Computes (A/'scale')^{-1/2} for A being symmetric, positive-semidefinite.
|
||||
// Eigenvalues of 'A' are clamped to be at least 'lower_eigenvalue_bound'.
|
||||
template <int N>
|
||||
Eigen::Matrix<double, N, N> ComputeSpdMatrixSqrtInverse(
|
||||
const Eigen::Matrix<double, N, N>& A, const double scale,
|
||||
const double lower_eigenvalue_bound) {
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>>
|
||||
covariance_eigen_solver(A / scale);
|
||||
if (covariance_eigen_solver.info() != Eigen::Success) {
|
||||
LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A;
|
||||
return Eigen::Matrix<double, N, N>::Identity();
|
||||
}
|
||||
// Since we compute the inverse, we do not allow smaller values to avoid
|
||||
// infinity and NaN.
|
||||
const double relative_lower_bound = lower_eigenvalue_bound / scale;
|
||||
return covariance_eigen_solver.eigenvectors() *
|
||||
covariance_eigen_solver.eigenvalues()
|
||||
.cwiseMax(relative_lower_bound)
|
||||
.cwiseInverse()
|
||||
.cwiseSqrt()
|
||||
.asDiagonal() *
|
||||
covariance_eigen_solver.eigenvectors().inverse();
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_MATH_H_
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_MUTEX_H_
|
||||
#define CARTOGRAPHER_COMMON_MUTEX_H_
|
||||
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// Enable thread safety attributes only with clang.
|
||||
// The attributes can be safely erased when compiling with other compilers.
|
||||
#if defined(__SUPPORT_TS_ANNOTATION__) || defined(__clang__)
|
||||
#define THREAD_ANNOTATION_ATTRIBUTE__(x) __attribute__((x))
|
||||
#else
|
||||
#define THREAD_ANNOTATION_ATTRIBUTE__(x) // no-op
|
||||
#endif
|
||||
|
||||
#define CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(capability(x))
|
||||
|
||||
#define SCOPED_CAPABILITY THREAD_ANNOTATION_ATTRIBUTE__(scoped_lockable)
|
||||
|
||||
#define GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(guarded_by(x))
|
||||
|
||||
#define PT_GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(pt_guarded_by(x))
|
||||
|
||||
#define REQUIRES(...) \
|
||||
THREAD_ANNOTATION_ATTRIBUTE__(requires_capability(__VA_ARGS__))
|
||||
|
||||
#define ACQUIRE(...) \
|
||||
THREAD_ANNOTATION_ATTRIBUTE__(acquire_capability(__VA_ARGS__))
|
||||
|
||||
#define RELEASE(...) \
|
||||
THREAD_ANNOTATION_ATTRIBUTE__(release_capability(__VA_ARGS__))
|
||||
|
||||
#define EXCLUDES(...) THREAD_ANNOTATION_ATTRIBUTE__(locks_excluded(__VA_ARGS__))
|
||||
|
||||
#define NO_THREAD_SAFETY_ANALYSIS \
|
||||
THREAD_ANNOTATION_ATTRIBUTE__(no_thread_safety_analysis)
|
||||
|
||||
// Defines an annotated mutex that can only be locked through its scoped locker
|
||||
// implementation.
|
||||
class CAPABILITY("mutex") Mutex {
|
||||
public:
|
||||
// A RAII class that acquires a mutex in its constructor, and
|
||||
// releases it in its destructor. It also implements waiting functionality on
|
||||
// conditions that get checked whenever the mutex is released.
|
||||
class SCOPED_CAPABILITY Locker {
|
||||
public:
|
||||
Locker(Mutex* mutex) ACQUIRE(mutex) : mutex_(mutex), lock_(mutex->mutex_) {}
|
||||
|
||||
~Locker() RELEASE() {
|
||||
lock_.unlock();
|
||||
mutex_->condition_.notify_all();
|
||||
}
|
||||
|
||||
template <typename Predicate>
|
||||
void Await(Predicate predicate) REQUIRES(this) {
|
||||
mutex_->condition_.wait(lock_, predicate);
|
||||
}
|
||||
|
||||
template <typename Predicate>
|
||||
bool AwaitWithTimeout(Predicate predicate, common::Duration timeout)
|
||||
REQUIRES(this) {
|
||||
return mutex_->condition_.wait_for(lock_, timeout, predicate);
|
||||
}
|
||||
|
||||
private:
|
||||
Mutex* mutex_;
|
||||
std::unique_lock<std::mutex> lock_;
|
||||
};
|
||||
|
||||
private:
|
||||
std::condition_variable condition_;
|
||||
std::mutex mutex_;
|
||||
};
|
||||
|
||||
using MutexLocker = Mutex::Locker;
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_MUTEX_H_
|
||||
|
|
@ -0,0 +1,179 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
|
||||
#define CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
|
||||
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
|
||||
#include "cartographer/common/blocking_queue.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/port.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// Number of items that can be queued up before we LOG(WARNING).
|
||||
const int kMaxQueueSize = 500;
|
||||
|
||||
// Maintains multiple queues of sorted values and dispatches merge sorted
|
||||
// values. This class is thread-compatible.
|
||||
template <typename QueueKeyType, typename SortKeyType, typename ValueType>
|
||||
class OrderedMultiQueue {
|
||||
public:
|
||||
using Callback = std::function<void(std::unique_ptr<ValueType>)>;
|
||||
|
||||
// Will wait to see at least one value for each 'expected_queue_keys' before
|
||||
// dispatching the next smallest value across all queues.
|
||||
explicit OrderedMultiQueue(const SortKeyType min_sort_key = SortKeyType())
|
||||
: last_dispatched_key_(min_sort_key) {}
|
||||
|
||||
~OrderedMultiQueue() {}
|
||||
|
||||
void AddQueue(const QueueKeyType& queue_key, Callback callback) {
|
||||
CHECK(FindOrNull(queue_key) == nullptr);
|
||||
queues_[queue_key].callback = callback;
|
||||
}
|
||||
|
||||
void MarkQueueAsFinished(const QueueKeyType& queue_key) {
|
||||
auto& queue = FindOrDie(queue_key);
|
||||
CHECK(!queue.finished);
|
||||
queue.finished = true;
|
||||
Dispatch();
|
||||
}
|
||||
|
||||
bool HasQueue(const QueueKeyType& queue_key) {
|
||||
return queues_.count(queue_key) != 0;
|
||||
}
|
||||
|
||||
void Add(const QueueKeyType& queue_key, const SortKeyType& sort_key,
|
||||
std::unique_ptr<ValueType> value) {
|
||||
auto* queue = FindOrNull(queue_key);
|
||||
if (queue == nullptr) {
|
||||
// TODO(damonkohler): This will not work for every value of "queue_key".
|
||||
LOG_EVERY_N(WARNING, 60) << "Ignored value for queue: '" << queue_key
|
||||
<< "'";
|
||||
return;
|
||||
}
|
||||
queue->queue.Push(
|
||||
common::make_unique<KeyValuePair>(sort_key, std::move(value)));
|
||||
Dispatch();
|
||||
}
|
||||
|
||||
// Dispatches all remaining values in sorted order and removes the underlying
|
||||
// queues.
|
||||
void Flush() {
|
||||
std::vector<QueueKeyType> unfinished_queues;
|
||||
for (auto& entry : queues_) {
|
||||
if (!entry.second.finished) {
|
||||
unfinished_queues.push_back(entry.first);
|
||||
}
|
||||
}
|
||||
for (auto& unfinished_queue : unfinished_queues) {
|
||||
MarkQueueAsFinished(unfinished_queue);
|
||||
}
|
||||
}
|
||||
|
||||
// Returns the number of available values associated with 'queue_key'.
|
||||
int num_available(const QueueKeyType& queue_key) {
|
||||
return FindOrDie(queue_key).queue.Size();
|
||||
}
|
||||
|
||||
private:
|
||||
struct KeyValuePair {
|
||||
KeyValuePair(const SortKeyType& sort_key, std::unique_ptr<ValueType> value)
|
||||
: sort_key(sort_key), value(std::move(value)) {}
|
||||
SortKeyType sort_key;
|
||||
std::unique_ptr<ValueType> value;
|
||||
};
|
||||
|
||||
struct Queue {
|
||||
common::BlockingQueue<std::unique_ptr<KeyValuePair>> queue;
|
||||
Callback callback;
|
||||
bool finished = false;
|
||||
};
|
||||
|
||||
// Returns the queue with 'key' or LOG(FATAL).
|
||||
Queue& FindOrDie(const QueueKeyType& key) {
|
||||
auto it = queues_.find(key);
|
||||
CHECK(it != queues_.end()) << "Did not find '" << key << "'.";
|
||||
return it->second;
|
||||
}
|
||||
|
||||
// Returns the queue with 'key' or nullptr.
|
||||
Queue* FindOrNull(const QueueKeyType& key) {
|
||||
auto it = queues_.find(key);
|
||||
if (it == queues_.end()) {
|
||||
return nullptr;
|
||||
}
|
||||
return &it->second;
|
||||
}
|
||||
|
||||
void Dispatch() {
|
||||
while (true) {
|
||||
Queue* next_queue = nullptr;
|
||||
const KeyValuePair* next_key_value_pair = nullptr;
|
||||
for (auto it = queues_.begin(); it != queues_.end();) {
|
||||
auto& queue = it->second.queue;
|
||||
const auto* key_value_pair = queue.template Peek<KeyValuePair>();
|
||||
if (key_value_pair == nullptr) {
|
||||
if (it->second.finished) {
|
||||
queues_.erase(it++);
|
||||
continue;
|
||||
}
|
||||
CannotMakeProgress();
|
||||
return;
|
||||
}
|
||||
if (next_key_value_pair == nullptr ||
|
||||
std::forward_as_tuple(key_value_pair->sort_key, it->first) <
|
||||
std::forward_as_tuple(next_key_value_pair->sort_key,
|
||||
it->first)) {
|
||||
next_key_value_pair = key_value_pair;
|
||||
next_queue = &it->second;
|
||||
}
|
||||
CHECK_LE(last_dispatched_key_, next_key_value_pair->sort_key)
|
||||
<< "Non-sorted values added to queue: '" << it->first << "'";
|
||||
++it;
|
||||
}
|
||||
if (next_key_value_pair == nullptr) {
|
||||
CHECK(queues_.empty());
|
||||
return;
|
||||
}
|
||||
last_dispatched_key_ = next_key_value_pair->sort_key;
|
||||
next_queue->callback(std::move(next_queue->queue.Pop()->value));
|
||||
}
|
||||
}
|
||||
|
||||
// Called when not all necessary queues are filled to dispatch messages.
|
||||
void CannotMakeProgress() {
|
||||
for (auto& entry : queues_) {
|
||||
LOG_IF_EVERY_N(WARNING, entry.second.queue.Size() > kMaxQueueSize, 60)
|
||||
<< "Queue " << entry.first << " exceeds maximum size.";
|
||||
}
|
||||
}
|
||||
|
||||
// Used to verify that values are dispatched in sorted order.
|
||||
SortKeyType last_dispatched_key_;
|
||||
|
||||
std::map<QueueKeyType, Queue> queues_;
|
||||
};
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/ordered_multi_queue.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
namespace {
|
||||
|
||||
TEST(OrderedMultiQueue, Ordering) {
|
||||
std::vector<int> values;
|
||||
OrderedMultiQueue<int, int, int> queue;
|
||||
for (int i : {1, 2, 3}) {
|
||||
queue.AddQueue(i, [&values](std::unique_ptr<int> value) {
|
||||
if (!values.empty()) {
|
||||
EXPECT_GT(*value, values.back());
|
||||
}
|
||||
values.push_back(*value);
|
||||
});
|
||||
}
|
||||
queue.Add(1, 4, common::make_unique<int>(4));
|
||||
queue.Add(1, 5, common::make_unique<int>(5));
|
||||
queue.Add(1, 6, common::make_unique<int>(6));
|
||||
EXPECT_TRUE(values.empty());
|
||||
queue.Add(2, 1, common::make_unique<int>(1));
|
||||
EXPECT_TRUE(values.empty());
|
||||
queue.Add(3, 2, common::make_unique<int>(2));
|
||||
EXPECT_EQ(values.size(), 1);
|
||||
queue.Add(2, 3, common::make_unique<int>(3));
|
||||
EXPECT_EQ(values.size(), 2);
|
||||
queue.Add(2, 7, common::make_unique<int>(7));
|
||||
queue.Add(3, 8, common::make_unique<int>(8));
|
||||
queue.Flush();
|
||||
|
||||
EXPECT_EQ(8, values.size());
|
||||
for (size_t i = 0; i < values.size(); ++i) {
|
||||
EXPECT_EQ(i + 1, values[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(OrderedMultiQueue, MarkQueueAsFinished) {
|
||||
std::vector<int> values;
|
||||
OrderedMultiQueue<int, int, int> queue;
|
||||
for (int i : {1, 2, 3}) {
|
||||
queue.AddQueue(i, [&values](std::unique_ptr<int> value) {
|
||||
if (!values.empty()) {
|
||||
EXPECT_GT(*value, values.back());
|
||||
}
|
||||
values.push_back(*value);
|
||||
});
|
||||
}
|
||||
queue.Add(1, 1, common::make_unique<int>(1));
|
||||
queue.Add(1, 2, common::make_unique<int>(2));
|
||||
queue.Add(1, 3, common::make_unique<int>(3));
|
||||
EXPECT_TRUE(values.empty());
|
||||
queue.MarkQueueAsFinished(1);
|
||||
EXPECT_TRUE(values.empty());
|
||||
queue.MarkQueueAsFinished(2);
|
||||
EXPECT_TRUE(values.empty());
|
||||
queue.MarkQueueAsFinished(3);
|
||||
|
||||
EXPECT_EQ(3, values.size());
|
||||
for (size_t i = 0; i < values.size(); ++i) {
|
||||
EXPECT_EQ(i + 1, values[i]);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_PORT_H_
|
||||
#define CARTOGRAPHER_COMMON_PORT_H_
|
||||
|
||||
#include <cinttypes>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
|
||||
#include <boost/iostreams/device/back_inserter.hpp>
|
||||
#include <boost/iostreams/filter/gzip.hpp>
|
||||
#include <boost/iostreams/filtering_stream.hpp>
|
||||
|
||||
using int8 = int8_t;
|
||||
using int16 = int16_t;
|
||||
using int32 = int32_t;
|
||||
using int64 = int64_t;
|
||||
using uint8 = uint8_t;
|
||||
using uint16 = uint16_t;
|
||||
using uint32 = uint32_t;
|
||||
using uint64 = uint64_t;
|
||||
|
||||
using std::string;
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
inline int RoundToInt(const float x) { return std::lround(x); }
|
||||
|
||||
inline int RoundToInt(const double x) { return std::lround(x); }
|
||||
|
||||
inline int64 RoundToInt64(const float x) { return std::lround(x); }
|
||||
|
||||
inline int64 RoundToInt64(const double x) { return std::lround(x); }
|
||||
|
||||
inline void FastGzipString(const string& uncompressed, string* compressed) {
|
||||
boost::iostreams::filtering_ostream out;
|
||||
out.push(
|
||||
boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));
|
||||
out.push(boost::iostreams::back_inserter(*compressed));
|
||||
boost::iostreams::write(out,
|
||||
reinterpret_cast<const char*>(uncompressed.data()),
|
||||
uncompressed.size());
|
||||
}
|
||||
|
||||
inline void FastGunzipString(const string& compressed, string* decompressed) {
|
||||
boost::iostreams::filtering_ostream out;
|
||||
out.push(boost::iostreams::gzip_decompressor());
|
||||
out.push(boost::iostreams::back_inserter(*decompressed));
|
||||
boost::iostreams::write(out, reinterpret_cast<const char*>(compressed.data()),
|
||||
compressed.size());
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_PORT_H_
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/thread_pool.h"
|
||||
|
||||
#include <unistd.h>
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <numeric>
|
||||
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
ThreadPool::ThreadPool(int num_threads) {
|
||||
MutexLocker locker(&mutex_);
|
||||
for (int i = 0; i != num_threads; ++i) {
|
||||
pool_.emplace_back([this]() { ThreadPool::DoWork(); });
|
||||
}
|
||||
}
|
||||
|
||||
ThreadPool::~ThreadPool() {
|
||||
{
|
||||
MutexLocker locker(&mutex_);
|
||||
CHECK(running_);
|
||||
running_ = false;
|
||||
CHECK_EQ(work_queue_.size(), 0);
|
||||
}
|
||||
for (std::thread& thread : pool_) {
|
||||
thread.join();
|
||||
}
|
||||
}
|
||||
|
||||
void ThreadPool::Schedule(std::function<void()> work_item) {
|
||||
MutexLocker locker(&mutex_);
|
||||
CHECK(running_);
|
||||
work_queue_.push_back(work_item);
|
||||
}
|
||||
|
||||
void ThreadPool::DoWork() {
|
||||
#ifdef __linux__
|
||||
// This changes the per-thread nice level of the current thread on Linux. We
|
||||
// do this so that the background work done by the thread pool is not taking
|
||||
// away CPU resources from more important foreground threads.
|
||||
CHECK_NE(nice(10), -1);
|
||||
#endif
|
||||
for (;;) {
|
||||
std::function<void()> work_item;
|
||||
{
|
||||
MutexLocker locker(&mutex_);
|
||||
locker.Await([this]() REQUIRES(mutex_) {
|
||||
return !work_queue_.empty() || !running_;
|
||||
});
|
||||
if (!work_queue_.empty()) {
|
||||
work_item = work_queue_.front();
|
||||
work_queue_.pop_front();
|
||||
} else if (!running_) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
CHECK(work_item);
|
||||
work_item();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_THREAD_POOL_H_
|
||||
#define CARTOGRAPHER_COMMON_THREAD_POOL_H_
|
||||
|
||||
#include <deque>
|
||||
#include <functional>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/mutex.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
// A fixed number of threads working on a work queue of work items. Adding a
|
||||
// new work item does not block, and will be executed by a background thread
|
||||
// eventually. The queue must be empty before calling the destructor. The thread
|
||||
// pool will then wait for the currently executing work items to finish and then
|
||||
// destroy the threads.
|
||||
class ThreadPool {
|
||||
public:
|
||||
explicit ThreadPool(int num_threads);
|
||||
~ThreadPool();
|
||||
|
||||
ThreadPool(const ThreadPool&) = delete;
|
||||
ThreadPool& operator=(const ThreadPool&) = delete;
|
||||
|
||||
void Schedule(std::function<void()> work_item);
|
||||
|
||||
private:
|
||||
void DoWork();
|
||||
|
||||
Mutex mutex_;
|
||||
bool running_ GUARDED_BY(mutex_) = true;
|
||||
std::vector<std::thread> pool_ GUARDED_BY(mutex_);
|
||||
std::deque<std::function<void()>> work_queue_ GUARDED_BY(mutex_);
|
||||
};
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_THREAD_POOL_H_
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
Duration FromSeconds(const double seconds) {
|
||||
return Duration(static_cast<int64>(1e7 * seconds));
|
||||
}
|
||||
|
||||
double ToSeconds(const Duration duration) { return duration.count() * 1e-7; }
|
||||
|
||||
Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); }
|
||||
|
||||
int64 ToUniversal(const Time time) { return time.time_since_epoch().count(); }
|
||||
|
||||
std::ostream& operator<<(std::ostream& os, const Time time) {
|
||||
os << std::to_string(ToUniversal(time));
|
||||
return os;
|
||||
}
|
||||
|
||||
common::Duration FromMilliseconds(int64 milliseconds) {
|
||||
return common::Duration(milliseconds * 10000);
|
||||
}
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_COMMON_TIME_H_
|
||||
#define CARTOGRAPHER_COMMON_TIME_H_
|
||||
|
||||
#include <chrono>
|
||||
#include <ostream>
|
||||
#include <ratio>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace common {
|
||||
|
||||
struct UniversalTimeScaleClock {
|
||||
using rep = int64;
|
||||
using period = std::ratio<1, 10000000>;
|
||||
using duration = std::chrono::duration<rep, period>;
|
||||
using time_point = std::chrono::time_point<UniversalTimeScaleClock>;
|
||||
static constexpr bool is_steady = true;
|
||||
};
|
||||
|
||||
// Represents Universal Time Scale durations and timestamps which are 64-bit
|
||||
// integers representing the 100 nanosecond ticks since the Epoch which is
|
||||
// January 1, 1 at the start of day in UTC.
|
||||
using Duration = UniversalTimeScaleClock::duration;
|
||||
using Time = UniversalTimeScaleClock::time_point;
|
||||
|
||||
// Convenience functions to create common::Durations.
|
||||
Duration FromSeconds(double seconds);
|
||||
Duration FromMilliseconds(int64 milliseconds);
|
||||
|
||||
// Returns the given duration in seconds.
|
||||
double ToSeconds(Duration duration);
|
||||
|
||||
// Creates a time from a Universal Time Scale.
|
||||
Time FromUniversal(int64 ticks);
|
||||
|
||||
// Outputs the Universal Time Scale timestamp for a given Time.
|
||||
int64 ToUniversal(Time time);
|
||||
|
||||
// For logging and unit tests, outputs the timestamp integer.
|
||||
std::ostream& operator<<(std::ostream& os, Time time);
|
||||
|
||||
} // namespace common
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_COMMON_TIME_H_
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
|
||||
#define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
|
||||
template <typename T, int N>
|
||||
class GaussianDistribution {
|
||||
public:
|
||||
GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean,
|
||||
const Eigen::Matrix<T, N, N>& covariance)
|
||||
: mean_(mean), covariance_(covariance) {}
|
||||
|
||||
const Eigen::Matrix<T, N, 1>& GetMean() const { return mean_; }
|
||||
|
||||
const Eigen::Matrix<T, N, N>& GetCovariance() const { return covariance_; }
|
||||
|
||||
private:
|
||||
Eigen::Matrix<T, N, 1> mean_;
|
||||
Eigen::Matrix<T, N, N> covariance_;
|
||||
};
|
||||
|
||||
template <typename T, int N>
|
||||
GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs,
|
||||
const GaussianDistribution<T, N>& rhs) {
|
||||
return GaussianDistribution<T, N>(lhs.GetMean() + rhs.GetMean(),
|
||||
lhs.GetCovariance() + rhs.GetCovariance());
|
||||
}
|
||||
|
||||
template <typename T, int N, int M>
|
||||
GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs,
|
||||
const GaussianDistribution<T, M>& rhs) {
|
||||
return GaussianDistribution<T, N>(
|
||||
lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose());
|
||||
}
|
||||
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/kalman_filter/gaussian_distribution.h"
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
namespace {
|
||||
|
||||
TEST(GaussianDistributionTest, testConstructor) {
|
||||
Eigen::Matrix2d covariance;
|
||||
covariance << 1., 2., 3., 4.;
|
||||
GaussianDistribution<double, 2> distribution(Eigen::Vector2d(0., 1.),
|
||||
covariance);
|
||||
EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);
|
||||
EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9);
|
||||
EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
|
||||
#define CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
|
||||
|
||||
#include <deque>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
|
||||
struct OdometryState {
|
||||
OdometryState(common::Time time, const transform::Rigid3d& odometer_pose,
|
||||
const transform::Rigid3d& state_pose);
|
||||
OdometryState() {}
|
||||
|
||||
common::Time time = common::Time::min();
|
||||
transform::Rigid3d odometer_pose = transform::Rigid3d::Identity();
|
||||
transform::Rigid3d state_pose = transform::Rigid3d::Identity();
|
||||
};
|
||||
|
||||
// Keeps track of the odometry states by keeping sliding window over some
|
||||
// number of them.
|
||||
class OdometryStateTracker {
|
||||
public:
|
||||
using OdometryStates = std::deque<OdometryState>;
|
||||
|
||||
explicit OdometryStateTracker(int window_size);
|
||||
|
||||
const OdometryStates& odometry_states() const;
|
||||
|
||||
// Adds a new 'odometry_state' and makes sure the maximum number of previous
|
||||
// odometry states is not exceeded.
|
||||
void AddOdometryState(const OdometryState& odometry_state);
|
||||
|
||||
// Returns true if no elements are present in the odometry queue.
|
||||
bool empty() const;
|
||||
|
||||
// Retrieves the most recent OdometryState or an empty one if non yet present.
|
||||
const OdometryState& newest() const;
|
||||
|
||||
private:
|
||||
OdometryStates odometry_states_;
|
||||
size_t window_size_;
|
||||
};
|
||||
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
|
||||
|
|
@ -0,0 +1,409 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <utility>
|
||||
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/kalman_filter/gaussian_distribution.h"
|
||||
#include "cartographer/kalman_filter/unscented_kalman_filter.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
|
||||
namespace {
|
||||
|
||||
PoseTracker::State AddDelta(const PoseTracker::State& state,
|
||||
const PoseTracker::State& delta) {
|
||||
PoseTracker::State new_state = state + delta;
|
||||
const Eigen::Quaterniond orientation =
|
||||
transform::AngleAxisVectorToRotationQuaternion(
|
||||
Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
|
||||
state[PoseTracker::kMapOrientationY],
|
||||
state[PoseTracker::kMapOrientationZ]));
|
||||
const Eigen::Vector3d rotation_vector(delta[PoseTracker::kMapOrientationX],
|
||||
delta[PoseTracker::kMapOrientationY],
|
||||
delta[PoseTracker::kMapOrientationZ]);
|
||||
CHECK_LT(rotation_vector.norm(), M_PI / 2.)
|
||||
<< "Sigma point is far from the mean, recovered delta may be incorrect.";
|
||||
const Eigen::Quaterniond rotation =
|
||||
transform::AngleAxisVectorToRotationQuaternion(rotation_vector);
|
||||
const Eigen::Vector3d new_orientation =
|
||||
transform::RotationQuaternionToAngleAxisVector(orientation * rotation);
|
||||
new_state[PoseTracker::kMapOrientationX] = new_orientation.x();
|
||||
new_state[PoseTracker::kMapOrientationY] = new_orientation.y();
|
||||
new_state[PoseTracker::kMapOrientationZ] = new_orientation.z();
|
||||
return new_state;
|
||||
}
|
||||
|
||||
PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
|
||||
const PoseTracker::State& target) {
|
||||
PoseTracker::State delta = target - origin;
|
||||
const Eigen::Quaterniond origin_orientation =
|
||||
transform::AngleAxisVectorToRotationQuaternion(
|
||||
Eigen::Vector3d(origin[PoseTracker::kMapOrientationX],
|
||||
origin[PoseTracker::kMapOrientationY],
|
||||
origin[PoseTracker::kMapOrientationZ]));
|
||||
const Eigen::Quaterniond target_orientation =
|
||||
transform::AngleAxisVectorToRotationQuaternion(
|
||||
Eigen::Vector3d(target[PoseTracker::kMapOrientationX],
|
||||
target[PoseTracker::kMapOrientationY],
|
||||
target[PoseTracker::kMapOrientationZ]));
|
||||
const Eigen::Vector3d rotation =
|
||||
transform::RotationQuaternionToAngleAxisVector(
|
||||
origin_orientation.inverse() * target_orientation);
|
||||
delta[PoseTracker::kMapOrientationX] = rotation.x();
|
||||
delta[PoseTracker::kMapOrientationY] = rotation.y();
|
||||
delta[PoseTracker::kMapOrientationZ] = rotation.z();
|
||||
return delta;
|
||||
}
|
||||
|
||||
// Build a model matrix for the given time delta.
|
||||
PoseTracker::State ModelFunction3D(const PoseTracker::State& state,
|
||||
const double delta_t) {
|
||||
CHECK_GT(delta_t, 0.);
|
||||
|
||||
PoseTracker::State new_state;
|
||||
new_state[PoseTracker::kMapPositionX] =
|
||||
state[PoseTracker::kMapPositionX] +
|
||||
delta_t * state[PoseTracker::kMapVelocityX];
|
||||
new_state[PoseTracker::kMapPositionY] =
|
||||
state[PoseTracker::kMapPositionY] +
|
||||
delta_t * state[PoseTracker::kMapVelocityY];
|
||||
new_state[PoseTracker::kMapPositionZ] =
|
||||
state[PoseTracker::kMapPositionZ] +
|
||||
delta_t * state[PoseTracker::kMapVelocityZ];
|
||||
|
||||
new_state[PoseTracker::kMapOrientationX] =
|
||||
state[PoseTracker::kMapOrientationX];
|
||||
new_state[PoseTracker::kMapOrientationY] =
|
||||
state[PoseTracker::kMapOrientationY];
|
||||
new_state[PoseTracker::kMapOrientationZ] =
|
||||
state[PoseTracker::kMapOrientationZ];
|
||||
|
||||
new_state[PoseTracker::kMapVelocityX] = state[PoseTracker::kMapVelocityX];
|
||||
new_state[PoseTracker::kMapVelocityY] = state[PoseTracker::kMapVelocityY];
|
||||
new_state[PoseTracker::kMapVelocityZ] = state[PoseTracker::kMapVelocityZ];
|
||||
|
||||
return new_state;
|
||||
}
|
||||
|
||||
// A specialization of ModelFunction3D that limits the z-component of position
|
||||
// and velocity to 0.
|
||||
PoseTracker::State ModelFunction2D(const PoseTracker::State& state,
|
||||
const double delta_t) {
|
||||
auto new_state = ModelFunction3D(state, delta_t);
|
||||
new_state[PoseTracker::kMapPositionZ] = 0.;
|
||||
new_state[PoseTracker::kMapVelocityZ] = 0.;
|
||||
new_state[PoseTracker::kMapOrientationX] = 0.;
|
||||
new_state[PoseTracker::kMapOrientationY] = 0.;
|
||||
return new_state;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
ImuTracker::ImuTracker(const proto::PoseTrackerOptions& options,
|
||||
const common::Time time)
|
||||
: options_(options),
|
||||
time_(time),
|
||||
last_linear_acceleration_time_(common::Time::min()),
|
||||
orientation_(Eigen::Quaterniond::Identity()),
|
||||
gravity_direction_(Eigen::Vector3d::UnitZ()),
|
||||
imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
|
||||
|
||||
void ImuTracker::Predict(const common::Time time) {
|
||||
CHECK_LE(time_, time);
|
||||
const double delta_t = common::ToSeconds(time - time_);
|
||||
const Eigen::Quaterniond rotation =
|
||||
transform::AngleAxisVectorToRotationQuaternion(
|
||||
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
|
||||
orientation_ = (orientation_ * rotation).normalized();
|
||||
gravity_direction_ = rotation.inverse() * gravity_direction_;
|
||||
time_ = time;
|
||||
}
|
||||
|
||||
void ImuTracker::AddImuLinearAccelerationObservation(
|
||||
const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
|
||||
Predict(time);
|
||||
// Update the 'gravity_direction_' with an exponential moving average using
|
||||
// the 'imu_gravity_time_constant'.
|
||||
const double delta_t =
|
||||
last_linear_acceleration_time_ > common::Time::min()
|
||||
? common::ToSeconds(time - last_linear_acceleration_time_)
|
||||
: std::numeric_limits<double>::infinity();
|
||||
last_linear_acceleration_time_ = time;
|
||||
const double alpha =
|
||||
1. - std::exp(-delta_t / options_.imu_gravity_time_constant());
|
||||
gravity_direction_ =
|
||||
(1. - alpha) * gravity_direction_ + alpha * imu_linear_acceleration;
|
||||
// Change the 'orientation_' so that it agrees with the current
|
||||
// 'gravity_direction_'.
|
||||
const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
|
||||
gravity_direction_, orientation_.inverse() * Eigen::Vector3d::UnitZ());
|
||||
orientation_ = (orientation_ * rotation).normalized();
|
||||
CHECK_GT((orientation_.inverse() * Eigen::Vector3d::UnitZ())
|
||||
.dot(gravity_direction_),
|
||||
0.);
|
||||
}
|
||||
|
||||
void ImuTracker::AddImuAngularVelocityObservation(
|
||||
const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
|
||||
Predict(time);
|
||||
imu_angular_velocity_ = imu_angular_velocity;
|
||||
}
|
||||
|
||||
PoseAndCovariance operator*(const transform::Rigid3d& transform,
|
||||
const PoseAndCovariance& pose_and_covariance) {
|
||||
GaussianDistribution<double, 6> distribution(
|
||||
Eigen::Matrix<double, 6, 1>::Zero(), pose_and_covariance.covariance);
|
||||
Eigen::Matrix<double, 6, 6> linear_transform;
|
||||
linear_transform << transform.rotation().matrix(), Eigen::Matrix3d::Zero(),
|
||||
Eigen::Matrix3d::Zero(), transform.rotation().matrix();
|
||||
return {transform * pose_and_covariance.pose,
|
||||
(linear_transform * distribution).GetCovariance()};
|
||||
}
|
||||
|
||||
proto::PoseTrackerOptions CreatePoseTrackerOptions(
|
||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||
proto::PoseTrackerOptions options;
|
||||
options.set_position_model_variance(
|
||||
parameter_dictionary->GetDouble("position_model_variance"));
|
||||
options.set_orientation_model_variance(
|
||||
parameter_dictionary->GetDouble("orientation_model_variance"));
|
||||
options.set_velocity_model_variance(
|
||||
parameter_dictionary->GetDouble("velocity_model_variance"));
|
||||
options.set_imu_gravity_time_constant(
|
||||
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
|
||||
options.set_imu_gravity_variance(
|
||||
parameter_dictionary->GetDouble("imu_gravity_variance"));
|
||||
options.set_num_odometry_states(
|
||||
parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
|
||||
CHECK_GT(options.num_odometry_states(), 0);
|
||||
return options;
|
||||
}
|
||||
|
||||
PoseTracker::Distribution PoseTracker::KalmanFilterInit() {
|
||||
State initial_state = State::Zero();
|
||||
// We are certain about the complete state at the beginning. We define the
|
||||
// initial pose to be at the origin and axis aligned. Additionally, we claim
|
||||
// that we are not moving.
|
||||
StateCovariance initial_covariance = 1e-9 * StateCovariance::Identity();
|
||||
return Distribution(initial_state, initial_covariance);
|
||||
}
|
||||
|
||||
PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
|
||||
const ModelFunction& model_function,
|
||||
const common::Time time)
|
||||
: options_(options),
|
||||
model_function_(model_function),
|
||||
time_(time),
|
||||
kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
|
||||
imu_tracker_(options, time),
|
||||
odometry_state_tracker_(options.num_odometry_states()) {}
|
||||
|
||||
PoseTracker::~PoseTracker() {}
|
||||
|
||||
PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) {
|
||||
Predict(time);
|
||||
return kalman_filter_.GetBelief();
|
||||
}
|
||||
|
||||
void PoseTracker::GetPoseEstimateMeanAndCovariance(const common::Time time,
|
||||
transform::Rigid3d* pose,
|
||||
PoseCovariance* covariance) {
|
||||
const Distribution belief = GetBelief(time);
|
||||
*pose = RigidFromState(belief.GetMean());
|
||||
static_assert(kMapPositionX == 0, "Cannot extract PoseCovariance.");
|
||||
static_assert(kMapPositionY == 1, "Cannot extract PoseCovariance.");
|
||||
static_assert(kMapPositionZ == 2, "Cannot extract PoseCovariance.");
|
||||
static_assert(kMapOrientationX == 3, "Cannot extract PoseCovariance.");
|
||||
static_assert(kMapOrientationY == 4, "Cannot extract PoseCovariance.");
|
||||
static_assert(kMapOrientationZ == 5, "Cannot extract PoseCovariance.");
|
||||
*covariance = belief.GetCovariance().block<6, 6>(0, 0);
|
||||
covariance->block<2, 2>(3, 3) +=
|
||||
options_.imu_gravity_variance() * Eigen::Matrix2d::Identity();
|
||||
}
|
||||
|
||||
const PoseTracker::Distribution PoseTracker::BuildModelNoise(
|
||||
const double delta_t) const {
|
||||
// Position is constant, but orientation changes.
|
||||
StateCovariance model_noise = StateCovariance::Zero();
|
||||
|
||||
model_noise.diagonal() <<
|
||||
// Position in map.
|
||||
options_.position_model_variance() * delta_t,
|
||||
options_.position_model_variance() * delta_t,
|
||||
options_.position_model_variance() * delta_t,
|
||||
|
||||
// Orientation in map.
|
||||
options_.orientation_model_variance() * delta_t,
|
||||
options_.orientation_model_variance() * delta_t,
|
||||
options_.orientation_model_variance() * delta_t,
|
||||
|
||||
// Linear velocities in map.
|
||||
options_.velocity_model_variance() * delta_t,
|
||||
options_.velocity_model_variance() * delta_t,
|
||||
options_.velocity_model_variance() * delta_t;
|
||||
|
||||
return Distribution(State::Zero(), model_noise);
|
||||
}
|
||||
|
||||
void PoseTracker::Predict(const common::Time time) {
|
||||
imu_tracker_.Predict(time);
|
||||
CHECK_LE(time_, time);
|
||||
const double delta_t = common::ToSeconds(time - time_);
|
||||
if (delta_t == 0.) {
|
||||
return;
|
||||
}
|
||||
kalman_filter_.Predict(
|
||||
[this, delta_t](const State& state) -> State {
|
||||
switch (model_function_) {
|
||||
case ModelFunction::k2D:
|
||||
return ModelFunction2D(state, delta_t);
|
||||
case ModelFunction::k3D:
|
||||
return ModelFunction3D(state, delta_t);
|
||||
default:
|
||||
LOG(FATAL);
|
||||
}
|
||||
},
|
||||
BuildModelNoise(delta_t));
|
||||
time_ = time;
|
||||
}
|
||||
|
||||
void PoseTracker::AddImuLinearAccelerationObservation(
|
||||
const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
|
||||
imu_tracker_.AddImuLinearAccelerationObservation(time,
|
||||
imu_linear_acceleration);
|
||||
Predict(time);
|
||||
}
|
||||
|
||||
void PoseTracker::AddImuAngularVelocityObservation(
|
||||
const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
|
||||
imu_tracker_.AddImuAngularVelocityObservation(time, imu_angular_velocity);
|
||||
Predict(time);
|
||||
}
|
||||
|
||||
void PoseTracker::AddPoseObservation(const common::Time time,
|
||||
const transform::Rigid3d& pose,
|
||||
const PoseCovariance& covariance) {
|
||||
Predict(time);
|
||||
|
||||
// Noise covariance is taken directly from the input values.
|
||||
const GaussianDistribution<double, 6> delta(
|
||||
Eigen::Matrix<double, 6, 1>::Zero(), covariance);
|
||||
|
||||
kalman_filter_.Observe<6>(
|
||||
[this, &pose](const State& state) -> Eigen::Matrix<double, 6, 1> {
|
||||
const transform::Rigid3d state_pose = RigidFromState(state);
|
||||
const Eigen::Vector3d delta_orientation =
|
||||
transform::RotationQuaternionToAngleAxisVector(
|
||||
pose.rotation().inverse() * state_pose.rotation());
|
||||
const Eigen::Vector3d delta_translation =
|
||||
state_pose.translation() - pose.translation();
|
||||
Eigen::Matrix<double, 6, 1> return_value;
|
||||
return_value << delta_translation, delta_orientation;
|
||||
return return_value;
|
||||
},
|
||||
delta);
|
||||
}
|
||||
|
||||
// Updates from the odometer are in the odometer's map-like frame, called the
|
||||
// 'odometry' frame. The odometer_pose converts data from the map frame
|
||||
// into the odometry frame.
|
||||
void PoseTracker::AddOdometerPoseObservation(
|
||||
const common::Time time, const transform::Rigid3d& odometer_pose,
|
||||
const PoseCovariance& covariance) {
|
||||
if (!odometry_state_tracker_.empty()) {
|
||||
const auto& previous_odometry_state = odometry_state_tracker_.newest();
|
||||
const transform::Rigid3d delta =
|
||||
previous_odometry_state.odometer_pose.inverse() * odometer_pose;
|
||||
const transform::Rigid3d new_pose =
|
||||
previous_odometry_state.state_pose * delta;
|
||||
AddPoseObservation(time, new_pose, covariance);
|
||||
}
|
||||
|
||||
const Distribution belief = GetBelief(time);
|
||||
|
||||
odometry_state_tracker_.AddOdometryState(
|
||||
{time, odometer_pose, RigidFromState(belief.GetMean())});
|
||||
}
|
||||
|
||||
const OdometryStateTracker::OdometryStates& PoseTracker::odometry_states()
|
||||
const {
|
||||
return odometry_state_tracker_.odometry_states();
|
||||
}
|
||||
|
||||
transform::Rigid3d PoseTracker::RigidFromState(
|
||||
const PoseTracker::State& state) {
|
||||
return transform::Rigid3d(
|
||||
Eigen::Vector3d(state[PoseTracker::kMapPositionX],
|
||||
state[PoseTracker::kMapPositionY],
|
||||
state[PoseTracker::kMapPositionZ]),
|
||||
transform::AngleAxisVectorToRotationQuaternion(
|
||||
Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
|
||||
state[PoseTracker::kMapOrientationY],
|
||||
state[PoseTracker::kMapOrientationZ])) *
|
||||
imu_tracker_.orientation());
|
||||
}
|
||||
|
||||
Pose2DCovariance Project2D(const PoseCovariance& covariance) {
|
||||
Pose2DCovariance projected_covariance;
|
||||
projected_covariance.block<2, 2>(0, 0) = covariance.block<2, 2>(0, 0);
|
||||
projected_covariance.block<2, 1>(0, 2) = covariance.block<2, 1>(0, 5);
|
||||
projected_covariance.block<1, 2>(2, 0) = covariance.block<1, 2>(5, 0);
|
||||
projected_covariance(2, 2) = covariance(5, 5);
|
||||
return projected_covariance;
|
||||
}
|
||||
|
||||
PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
|
||||
const double position_variance,
|
||||
const double orientation_variance) {
|
||||
PoseCovariance covariance;
|
||||
covariance.setZero();
|
||||
covariance.block<2, 2>(0, 0) = embedded_covariance.block<2, 2>(0, 0);
|
||||
covariance.block<2, 1>(0, 5) = embedded_covariance.block<2, 1>(0, 2);
|
||||
covariance.block<1, 2>(5, 0) = embedded_covariance.block<1, 2>(2, 0);
|
||||
covariance(5, 5) = embedded_covariance(2, 2);
|
||||
|
||||
covariance(2, 2) = position_variance;
|
||||
covariance(3, 3) = orientation_variance;
|
||||
covariance(4, 4) = orientation_variance;
|
||||
return covariance;
|
||||
}
|
||||
|
||||
PoseCovariance PoseCovarianceFromProtoMatrix(
|
||||
const sensor::proto::Matrix& proto_matrix) {
|
||||
PoseCovariance covariance;
|
||||
CHECK_EQ(proto_matrix.rows(), 6);
|
||||
CHECK_EQ(proto_matrix.cols(), 6);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
for (int j = 0; j < 6; ++j) {
|
||||
covariance(i, j) = proto_matrix.data(i * 6 + j);
|
||||
}
|
||||
}
|
||||
return covariance;
|
||||
}
|
||||
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,176 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
|
||||
#define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
|
||||
|
||||
#include <deque>
|
||||
#include <memory>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Core"
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/kalman_filter/gaussian_distribution.h"
|
||||
#include "cartographer/kalman_filter/odometry_state_tracker.h"
|
||||
#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
|
||||
#include "cartographer/kalman_filter/unscented_kalman_filter.h"
|
||||
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
|
||||
typedef Eigen::Matrix3d Pose2DCovariance;
|
||||
typedef Eigen::Matrix<double, 6, 6> PoseCovariance;
|
||||
|
||||
struct PoseAndCovariance {
|
||||
transform::Rigid3d pose;
|
||||
PoseCovariance covariance;
|
||||
};
|
||||
|
||||
PoseAndCovariance operator*(const transform::Rigid3d& transform,
|
||||
const PoseAndCovariance& pose_and_covariance);
|
||||
|
||||
// Projects a 3D pose covariance into a 2D pose covariance.
|
||||
Pose2DCovariance Project2D(const PoseCovariance& embedded_covariance);
|
||||
|
||||
// Embeds a 2D pose covariance into a 3D pose covariance.
|
||||
PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
|
||||
double position_variance, double orientation_variance);
|
||||
|
||||
// Deserializes the 'proto_matrix' into a PoseCovariance.
|
||||
PoseCovariance PoseCovarianceFromProtoMatrix(
|
||||
const sensor::proto::Matrix& proto_matrix);
|
||||
|
||||
proto::PoseTrackerOptions CreatePoseTrackerOptions(
|
||||
common::LuaParameterDictionary* parameter_dictionary);
|
||||
|
||||
// Keeps track of the orientation using angular velocities and linear
|
||||
// accelerations from an IMU. Because averaged linear acceleration (assuming
|
||||
// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
|
||||
// though yaw does.
|
||||
class ImuTracker {
|
||||
public:
|
||||
ImuTracker(const proto::PoseTrackerOptions& options, common::Time time);
|
||||
|
||||
// Updates the orientation to reflect the given 'time'.
|
||||
void Predict(common::Time time);
|
||||
|
||||
// Updates from an IMU reading (in the IMU frame).
|
||||
void AddImuLinearAccelerationObservation(
|
||||
common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
|
||||
void AddImuAngularVelocityObservation(
|
||||
common::Time time, const Eigen::Vector3d& imu_angular_velocity);
|
||||
|
||||
// Query the current orientation estimate.
|
||||
Eigen::Quaterniond orientation() { return orientation_; }
|
||||
|
||||
private:
|
||||
const proto::PoseTrackerOptions options_;
|
||||
common::Time time_;
|
||||
common::Time last_linear_acceleration_time_;
|
||||
Eigen::Quaterniond orientation_;
|
||||
Eigen::Vector3d gravity_direction_;
|
||||
Eigen::Vector3d imu_angular_velocity_;
|
||||
};
|
||||
|
||||
// A Kalman filter for a 3D state of position and orientation.
|
||||
// Includes functions to update from IMU and laser scan matches.
|
||||
class PoseTracker {
|
||||
public:
|
||||
enum {
|
||||
kMapPositionX = 0,
|
||||
kMapPositionY,
|
||||
kMapPositionZ,
|
||||
kMapOrientationX,
|
||||
kMapOrientationY,
|
||||
kMapOrientationZ,
|
||||
kMapVelocityX,
|
||||
kMapVelocityY,
|
||||
kMapVelocityZ,
|
||||
kDimension // We terminate loops with this.
|
||||
};
|
||||
|
||||
enum class ModelFunction { k2D, k3D };
|
||||
|
||||
using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;
|
||||
using State = KalmanFilter::StateType;
|
||||
using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;
|
||||
using Distribution = GaussianDistribution<double, kDimension>;
|
||||
|
||||
// Create a new Kalman filter starting at the origin with a standard normal
|
||||
// distribution at 'time'.
|
||||
PoseTracker(const proto::PoseTrackerOptions& options,
|
||||
const ModelFunction& model_function, common::Time time);
|
||||
virtual ~PoseTracker();
|
||||
|
||||
// Sets 'pose' and 'covariance' to the mean and covariance of the belief at
|
||||
// 'time' which must be >= to the current time. Must not be nullptr.
|
||||
void GetPoseEstimateMeanAndCovariance(common::Time time,
|
||||
transform::Rigid3d* pose,
|
||||
PoseCovariance* covariance);
|
||||
|
||||
// Updates from an IMU reading (in the IMU frame).
|
||||
void AddImuLinearAccelerationObservation(
|
||||
common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
|
||||
void AddImuAngularVelocityObservation(
|
||||
common::Time time, const Eigen::Vector3d& imu_angular_velocity);
|
||||
|
||||
// Updates from a pose estimate in the map frame.
|
||||
void AddPoseObservation(common::Time time, const transform::Rigid3d& pose,
|
||||
const PoseCovariance& covariance);
|
||||
|
||||
// Updates from a pose estimate in the odometer's map-like frame.
|
||||
void AddOdometerPoseObservation(common::Time time,
|
||||
const transform::Rigid3d& pose,
|
||||
const PoseCovariance& covariance);
|
||||
|
||||
common::Time time() const { return time_; }
|
||||
|
||||
// Returns the belief at the 'time' which must be >= to the current time.
|
||||
Distribution GetBelief(common::Time time);
|
||||
|
||||
const OdometryStateTracker::OdometryStates& odometry_states() const;
|
||||
|
||||
private:
|
||||
// Returns the distribution required to initialize the KalmanFilter.
|
||||
static Distribution KalmanFilterInit();
|
||||
|
||||
// Build a model noise distribution (zero mean) for the given time delta.
|
||||
const Distribution BuildModelNoise(double delta_t) const;
|
||||
|
||||
// Predict the state forward in time. This is a no-op if 'time' has not moved
|
||||
// forward.
|
||||
void Predict(common::Time time);
|
||||
|
||||
// Computes a pose combining the given 'state' with the 'imu_tracker_'
|
||||
// orientation.
|
||||
transform::Rigid3d RigidFromState(const PoseTracker::State& state);
|
||||
|
||||
const proto::PoseTrackerOptions options_;
|
||||
const ModelFunction model_function_;
|
||||
common::Time time_;
|
||||
KalmanFilter kalman_filter_;
|
||||
ImuTracker imu_tracker_;
|
||||
OdometryStateTracker odometry_state_tracker_;
|
||||
};
|
||||
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
|
||||
|
|
@ -0,0 +1,256 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||
|
||||
#include <random>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/rigid_transform_test_helpers.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
namespace {
|
||||
|
||||
constexpr double kOdometerVariance = 1e-12;
|
||||
|
||||
using transform::IsNearly;
|
||||
using transform::Rigid3d;
|
||||
using ::testing::Not;
|
||||
|
||||
class PoseTrackerTest : public ::testing::Test {
|
||||
protected:
|
||||
PoseTrackerTest() {
|
||||
auto parameter_dictionary = common::MakeDictionary(R"text(
|
||||
return {
|
||||
orientation_model_variance = 1e-8,
|
||||
position_model_variance = 1e-8,
|
||||
velocity_model_variance = 1e-8,
|
||||
imu_gravity_time_constant = 100.,
|
||||
imu_gravity_variance = 1e-9,
|
||||
num_odometry_states = 1,
|
||||
}
|
||||
)text");
|
||||
const proto::PoseTrackerOptions options =
|
||||
CreatePoseTrackerOptions(parameter_dictionary.get());
|
||||
pose_tracker_ = common::make_unique<PoseTracker>(
|
||||
options, PoseTracker::ModelFunction::k3D, common::FromUniversal(1000));
|
||||
}
|
||||
|
||||
std::unique_ptr<PoseTracker> pose_tracker_;
|
||||
};
|
||||
|
||||
TEST(CovarianceTest, EmbedAndProjectCovariance) {
|
||||
std::mt19937 prng(42);
|
||||
std::uniform_real_distribution<float> distribution(-10.f, 10.f);
|
||||
for (int i = 0; i < 100; ++i) {
|
||||
Pose2DCovariance covariance;
|
||||
for (int row = 0; row < 3; ++row) {
|
||||
for (int column = 0; column < 3; ++column) {
|
||||
covariance(row, column) = distribution(prng);
|
||||
}
|
||||
}
|
||||
const PoseCovariance embedded_covariance =
|
||||
Embed3D(covariance, distribution(prng), distribution(prng));
|
||||
EXPECT_TRUE(
|
||||
(Project2D(embedded_covariance).array() == covariance.array()).all());
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(PoseTrackerTest, SaveAndRestore) {
|
||||
std::vector<Rigid3d> poses(3);
|
||||
std::vector<PoseCovariance> covariances(3);
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500),
|
||||
&poses[0], &covariances[0]);
|
||||
|
||||
pose_tracker_->AddImuLinearAccelerationObservation(
|
||||
common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
|
||||
|
||||
PoseTracker copy_of_pose_tracker = *pose_tracker_;
|
||||
|
||||
const Eigen::Vector3d observation(2, 0, 8);
|
||||
pose_tracker_->AddImuLinearAccelerationObservation(
|
||||
common::FromUniversal(3000), observation);
|
||||
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(3500),
|
||||
&poses[1], &covariances[1]);
|
||||
|
||||
copy_of_pose_tracker.AddImuLinearAccelerationObservation(
|
||||
common::FromUniversal(3000), observation);
|
||||
copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance(
|
||||
common::FromUniversal(3500), &poses[2], &covariances[2]);
|
||||
|
||||
EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
|
||||
EXPECT_FALSE((covariances[0].array() == covariances[1].array()).all());
|
||||
EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
|
||||
EXPECT_TRUE((covariances[1].array() == covariances[2].array()).all());
|
||||
}
|
||||
|
||||
TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
|
||||
auto time = common::FromUniversal(1000);
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
time += std::chrono::seconds(5);
|
||||
pose_tracker_->AddImuLinearAccelerationObservation(
|
||||
time, Eigen::Vector3d(0., 0., 10.));
|
||||
}
|
||||
|
||||
{
|
||||
Rigid3d pose;
|
||||
PoseCovariance covariance;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
|
||||
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
|
||||
const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
|
||||
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
|
||||
<< actual.coeffs();
|
||||
}
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
time += std::chrono::seconds(5);
|
||||
pose_tracker_->AddImuLinearAccelerationObservation(
|
||||
time, Eigen::Vector3d(0., 10., 0.));
|
||||
}
|
||||
|
||||
time += std::chrono::milliseconds(5);
|
||||
|
||||
Rigid3d pose;
|
||||
PoseCovariance covariance;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
|
||||
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
|
||||
const Eigen::Quaterniond expected = Eigen::Quaterniond(
|
||||
Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
|
||||
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
|
||||
<< actual.coeffs();
|
||||
}
|
||||
|
||||
TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
|
||||
auto time = common::FromUniversal(1000);
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
time += std::chrono::milliseconds(5);
|
||||
pose_tracker_->AddImuAngularVelocityObservation(time,
|
||||
Eigen::Vector3d::Zero());
|
||||
}
|
||||
|
||||
{
|
||||
Rigid3d pose;
|
||||
PoseCovariance covariance;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
|
||||
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
|
||||
const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
|
||||
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
|
||||
<< actual.coeffs();
|
||||
}
|
||||
|
||||
const double target_radians = M_PI / 2.;
|
||||
const double num_observations = 300.;
|
||||
const double angular_velocity = target_radians / (num_observations * 5e-3);
|
||||
for (int i = 0; i < num_observations; ++i) {
|
||||
time += std::chrono::milliseconds(5);
|
||||
pose_tracker_->AddImuAngularVelocityObservation(
|
||||
time, Eigen::Vector3d(angular_velocity, 0., 0.));
|
||||
}
|
||||
|
||||
time += std::chrono::milliseconds(5);
|
||||
|
||||
Rigid3d pose;
|
||||
PoseCovariance covariance;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
|
||||
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
|
||||
const Eigen::Quaterniond expected = Eigen::Quaterniond(
|
||||
Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
|
||||
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
|
||||
<< actual.coeffs();
|
||||
}
|
||||
|
||||
TEST_F(PoseTrackerTest, AddPoseObservation) {
|
||||
auto time = common::FromUniversal(1000);
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
time += std::chrono::milliseconds(5);
|
||||
pose_tracker_->AddPoseObservation(
|
||||
time, Rigid3d::Identity(),
|
||||
Eigen::Matrix<double, 6, 6>::Identity() * 1e-6);
|
||||
}
|
||||
|
||||
{
|
||||
Rigid3d actual;
|
||||
PoseCovariance covariance;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
|
||||
EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
|
||||
}
|
||||
|
||||
const Rigid3d expected =
|
||||
Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
|
||||
Rigid3d::Rotation(Eigen::AngleAxisd(
|
||||
M_PI / 2., Eigen::Vector3d(0., 0., 3.).normalized()));
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
time += std::chrono::milliseconds(15);
|
||||
pose_tracker_->AddPoseObservation(
|
||||
time, expected, Eigen::Matrix<double, 6, 6>::Identity() * 1e-9);
|
||||
}
|
||||
|
||||
time += std::chrono::milliseconds(15);
|
||||
|
||||
Rigid3d actual;
|
||||
PoseCovariance covariance;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
|
||||
EXPECT_THAT(actual, IsNearly(expected, 1e-3));
|
||||
}
|
||||
|
||||
TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
|
||||
common::Time time = common::FromUniversal(0);
|
||||
|
||||
std::vector<Rigid3d> odometer_track;
|
||||
odometer_track.push_back(Rigid3d::Identity());
|
||||
odometer_track.push_back(
|
||||
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
|
||||
odometer_track.push_back(
|
||||
Rigid3d::Translation(Eigen::Vector3d(0.2, 0., 0.)) *
|
||||
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
|
||||
odometer_track.push_back(
|
||||
Rigid3d::Translation(Eigen::Vector3d(0.3, 0.1, 0.)) *
|
||||
Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
|
||||
odometer_track.push_back(
|
||||
Rigid3d::Translation(Eigen::Vector3d(0.2, 0.2, 0.1)) *
|
||||
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
|
||||
odometer_track.push_back(
|
||||
Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.2)) *
|
||||
Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
|
||||
odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
|
||||
|
||||
Rigid3d actual;
|
||||
PoseCovariance unused_covariance;
|
||||
for (const Rigid3d& pose : odometer_track) {
|
||||
time += std::chrono::seconds(1);
|
||||
pose_tracker_->AddOdometerPoseObservation(
|
||||
time, pose, kOdometerVariance * PoseCovariance::Identity());
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual,
|
||||
&unused_covariance);
|
||||
EXPECT_THAT(actual, IsNearly(pose, 1e-2));
|
||||
}
|
||||
// Sanity check that the test has signal:
|
||||
EXPECT_THAT(actual, Not(IsNearly(odometer_track[0], 1e-2)));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,291 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
|
||||
#define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <vector>
|
||||
|
||||
#include "Eigen/Cholesky"
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Eigenvalues"
|
||||
#include "cartographer/kalman_filter/gaussian_distribution.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
|
||||
template <typename FloatType>
|
||||
constexpr FloatType sqr(FloatType a) {
|
||||
return a * a;
|
||||
}
|
||||
|
||||
template <typename FloatType, int N>
|
||||
Eigen::Matrix<FloatType, N, N> OuterProduct(
|
||||
const Eigen::Matrix<FloatType, N, 1>& v) {
|
||||
return v * v.transpose();
|
||||
}
|
||||
|
||||
// Checks if 'A' is a symmetric matrix.
|
||||
template <typename FloatType, int N>
|
||||
void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A) {
|
||||
// This should be pretty much Eigen::Matrix<>::Zero() if the matrix is
|
||||
// symmetric.
|
||||
const FloatType norm = (A - A.transpose()).norm();
|
||||
CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5)
|
||||
<< "Symmetry check failed with norm: '" << norm << "' from matrix:\n"
|
||||
<< A;
|
||||
}
|
||||
|
||||
// Returns the matrix square root of a symmetric positive semidefinite matrix.
|
||||
template <typename FloatType, int N>
|
||||
Eigen::Matrix<FloatType, N, N> MatrixSqrt(
|
||||
const Eigen::Matrix<FloatType, N, N>& A) {
|
||||
CheckSymmetric(A);
|
||||
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FloatType, N, N>>
|
||||
adjoint_eigen_solver((A + A.transpose()) / 2.);
|
||||
const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();
|
||||
CHECK_GT(eigenvalues.minCoeff(), -1e-5)
|
||||
<< "MatrixSqrt failed with negative eigenvalues: "
|
||||
<< eigenvalues.transpose();
|
||||
|
||||
return adjoint_eigen_solver.eigenvectors() *
|
||||
adjoint_eigen_solver.eigenvalues()
|
||||
.cwiseMax(Eigen::Matrix<FloatType, N, 1>::Zero())
|
||||
.cwiseSqrt()
|
||||
.asDiagonal() *
|
||||
adjoint_eigen_solver.eigenvectors().transpose();
|
||||
}
|
||||
|
||||
// Implementation of a Kalman filter. We follow the nomenclature from
|
||||
// Thrun, S. et al., Probabilistic Robotics, 2006.
|
||||
//
|
||||
// Extended to handle non-additive noise/sensors inspired by Kraft, E., A
|
||||
// Quaternion-based Unscented Kalman Filter for Orientation Tracking.
|
||||
template <typename FloatType, int N>
|
||||
class UnscentedKalmanFilter {
|
||||
public:
|
||||
using StateType = Eigen::Matrix<FloatType, N, 1>;
|
||||
using StateCovarianceType = Eigen::Matrix<FloatType, N, N>;
|
||||
|
||||
explicit UnscentedKalmanFilter(
|
||||
const GaussianDistribution<FloatType, N>& initial_belief,
|
||||
std::function<StateType(const StateType& state, const StateType& delta)>
|
||||
add_delta = [](const StateType& state,
|
||||
const StateType& delta) { return state + delta; },
|
||||
std::function<StateType(const StateType& origin, const StateType& target)>
|
||||
compute_delta =
|
||||
[](const StateType& origin, const StateType& target) {
|
||||
return target - origin;
|
||||
})
|
||||
: belief_(initial_belief),
|
||||
add_delta_(add_delta),
|
||||
compute_delta_(compute_delta) {}
|
||||
|
||||
// Does the control/prediction step for the filter. The control must be
|
||||
// implicitly added by the function g which also does the state transition.
|
||||
// 'epsilon' is the additive combination of control and model noise.
|
||||
void Predict(std::function<StateType(const StateType&)> g,
|
||||
const GaussianDistribution<FloatType, N>& epsilon) {
|
||||
CheckSymmetric(epsilon.GetCovariance());
|
||||
|
||||
// Get the state mean and matrix root of its covariance.
|
||||
const StateType& mu = belief_.GetMean();
|
||||
const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
|
||||
|
||||
std::vector<StateType> Y;
|
||||
Y.reserve(2 * N + 1);
|
||||
Y.emplace_back(g(mu));
|
||||
|
||||
const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
|
||||
for (int i = 0; i < N; ++i) {
|
||||
// Order does not matter here as all have the same weights in the
|
||||
// summation later on anyways.
|
||||
Y.emplace_back(g(add_delta_(mu, kSqrtNPlusLambda * sqrt_sigma.col(i))));
|
||||
Y.emplace_back(g(add_delta_(mu, -kSqrtNPlusLambda * sqrt_sigma.col(i))));
|
||||
}
|
||||
const StateType new_mu = ComputeMean(Y);
|
||||
|
||||
StateCovarianceType new_sigma =
|
||||
kCovWeight0 * OuterProduct<FloatType, N>(compute_delta_(new_mu, Y[0]));
|
||||
for (int i = 0; i < N; ++i) {
|
||||
new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
|
||||
compute_delta_(new_mu, Y[2 * i + 1]));
|
||||
new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
|
||||
compute_delta_(new_mu, Y[2 * i + 2]));
|
||||
}
|
||||
CheckSymmetric(new_sigma);
|
||||
|
||||
belief_ = GaussianDistribution<FloatType, N>(new_mu, new_sigma) + epsilon;
|
||||
}
|
||||
|
||||
// The observation step of the Kalman filter. 'h' transfers the state
|
||||
// into an observation that should be zero, i.e., the sensor readings should
|
||||
// be included in this function already. 'delta' is the measurement noise and
|
||||
// must have zero mean.
|
||||
template <int K>
|
||||
void Observe(
|
||||
std::function<Eigen::Matrix<FloatType, K, 1>(const StateType&)> h,
|
||||
const GaussianDistribution<FloatType, K>& delta) {
|
||||
CheckSymmetric(delta.GetCovariance());
|
||||
// We expect zero mean delta.
|
||||
CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9);
|
||||
|
||||
// Get the state mean and matrix root of its covariance.
|
||||
const StateType& mu = belief_.GetMean();
|
||||
const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
|
||||
|
||||
// As in Kraft's paper, we compute W containing the zero-mean sigma points,
|
||||
// since this is all we need.
|
||||
std::vector<StateType> W;
|
||||
W.reserve(2 * N + 1);
|
||||
W.emplace_back(StateType::Zero());
|
||||
|
||||
std::vector<Eigen::Matrix<FloatType, K, 1>> Z;
|
||||
Z.reserve(2 * N + 1);
|
||||
Z.emplace_back(h(mu));
|
||||
|
||||
Eigen::Matrix<FloatType, K, 1> z_hat = kMeanWeight0 * Z[0];
|
||||
const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
|
||||
for (int i = 0; i < N; ++i) {
|
||||
// Order does not matter here as all have the same weights in the
|
||||
// summation later on anyways.
|
||||
W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i));
|
||||
Z.emplace_back(h(add_delta_(mu, W.back())));
|
||||
|
||||
W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i));
|
||||
Z.emplace_back(h(add_delta_(mu, W.back())));
|
||||
|
||||
z_hat += kMeanWeightI * Z[2 * i + 1];
|
||||
z_hat += kMeanWeightI * Z[2 * i + 2];
|
||||
}
|
||||
|
||||
Eigen::Matrix<FloatType, K, K> S =
|
||||
kCovWeight0 * OuterProduct<FloatType, K>(Z[0] - z_hat);
|
||||
for (int i = 0; i < N; ++i) {
|
||||
S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 1] - z_hat);
|
||||
S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 2] - z_hat);
|
||||
}
|
||||
CheckSymmetric(S);
|
||||
S += delta.GetCovariance();
|
||||
|
||||
Eigen::Matrix<FloatType, N, K> sigma_bar_xz =
|
||||
kCovWeight0 * W[0] * (Z[0] - z_hat).transpose();
|
||||
for (int i = 0; i < N; ++i) {
|
||||
sigma_bar_xz +=
|
||||
kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose();
|
||||
sigma_bar_xz +=
|
||||
kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose();
|
||||
}
|
||||
|
||||
const Eigen::Matrix<FloatType, N, K> kalman_gain =
|
||||
sigma_bar_xz * S.inverse();
|
||||
const StateCovarianceType new_sigma =
|
||||
belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose();
|
||||
CheckSymmetric(new_sigma);
|
||||
|
||||
belief_ = GaussianDistribution<FloatType, N>(
|
||||
add_delta_(mu, kalman_gain * -z_hat), new_sigma);
|
||||
}
|
||||
|
||||
const GaussianDistribution<FloatType, N>& GetBelief() const {
|
||||
return belief_;
|
||||
}
|
||||
|
||||
private:
|
||||
StateType ComputeWeightedError(const StateType& mean_estimate,
|
||||
const std::vector<StateType>& states) {
|
||||
StateType weighted_error =
|
||||
kMeanWeight0 * compute_delta_(mean_estimate, states[0]);
|
||||
for (int i = 1; i != 2 * N + 1; ++i) {
|
||||
weighted_error += kMeanWeightI * compute_delta_(mean_estimate, states[i]);
|
||||
}
|
||||
return weighted_error;
|
||||
}
|
||||
|
||||
// Algorithm for computing the mean of non-additive states taken from Kraft's
|
||||
// Section 3.4, adapted to our implementation.
|
||||
StateType ComputeMean(const std::vector<StateType>& states) {
|
||||
CHECK_EQ(states.size(), 2 * N + 1);
|
||||
StateType current_estimate = states[0];
|
||||
StateType weighted_error = ComputeWeightedError(current_estimate, states);
|
||||
int iterations = 0;
|
||||
while (weighted_error.norm() > 1e-9) {
|
||||
double step_size = 1.;
|
||||
while (true) {
|
||||
const StateType next_estimate =
|
||||
add_delta_(current_estimate, step_size * weighted_error);
|
||||
const StateType next_error =
|
||||
ComputeWeightedError(next_estimate, states);
|
||||
if (next_error.norm() < weighted_error.norm()) {
|
||||
current_estimate = next_estimate;
|
||||
weighted_error = next_error;
|
||||
break;
|
||||
}
|
||||
step_size *= 0.5;
|
||||
CHECK_GT(step_size, 1e-3) << "Step size too small, line search failed.";
|
||||
}
|
||||
++iterations;
|
||||
CHECK_LT(iterations, 20) << "Too many iterations.";
|
||||
}
|
||||
return current_estimate;
|
||||
}
|
||||
|
||||
// According to Wikipedia these are the normal values. Thrun does not
|
||||
// mention those.
|
||||
constexpr static FloatType kAlpha = 1e-3;
|
||||
constexpr static FloatType kKappa = 0.;
|
||||
constexpr static FloatType kBeta = 2.;
|
||||
constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;
|
||||
constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);
|
||||
constexpr static FloatType kCovWeight0 =
|
||||
kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);
|
||||
constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));
|
||||
constexpr static FloatType kCovWeightI = kMeanWeightI;
|
||||
|
||||
GaussianDistribution<FloatType, N> belief_;
|
||||
const std::function<StateType(const StateType& state, const StateType& delta)>
|
||||
add_delta_;
|
||||
const std::function<StateType(const StateType& origin,
|
||||
const StateType& target)>
|
||||
compute_delta_;
|
||||
};
|
||||
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kAlpha;
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kKappa;
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kBeta;
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kLambda;
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeight0;
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeight0;
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeightI;
|
||||
template <typename FloatType, int N>
|
||||
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeightI;
|
||||
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/kalman_filter/unscented_kalman_filter.h"
|
||||
|
||||
#include "cartographer/kalman_filter/gaussian_distribution.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace kalman_filter {
|
||||
namespace {
|
||||
|
||||
Eigen::Matrix<double, 1, 1> Scalar(double value) {
|
||||
return value * Eigen::Matrix<double, 1, 1>::Identity();
|
||||
}
|
||||
|
||||
// A simple model that copies the first to the second state variable.
|
||||
Eigen::Matrix<double, 2, 1> g(const Eigen::Matrix<double, 2, 1>& state) {
|
||||
Eigen::Matrix<double, 2, 1> new_state;
|
||||
new_state << state[0], state[0];
|
||||
return new_state;
|
||||
}
|
||||
|
||||
// A simple observation of the first state variable.
|
||||
Eigen::Matrix<double, 1, 1> h(const Eigen::Matrix<double, 2, 1>& state) {
|
||||
return Scalar(state[0]) - Scalar(5.);
|
||||
}
|
||||
|
||||
TEST(KalmanFilterTest, testConstructor) {
|
||||
UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
|
||||
Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
|
||||
EXPECT_NEAR(42., filter.GetBelief().GetMean()[1], 1e-9);
|
||||
}
|
||||
|
||||
TEST(KalmanFilterTest, testPredict) {
|
||||
UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
|
||||
Eigen::Vector2d(42., 0.), 10. * Eigen::Matrix2d::Identity()));
|
||||
filter.Predict(
|
||||
g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
|
||||
Eigen::Matrix2d::Identity() * 1e-9));
|
||||
EXPECT_NEAR(filter.GetBelief().GetMean()[0], 42., 1e-2);
|
||||
EXPECT_NEAR(filter.GetBelief().GetMean()[1], 42., 1e-2);
|
||||
}
|
||||
|
||||
TEST(KalmanFilterTest, testObserve) {
|
||||
UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
|
||||
Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
|
||||
for (int i = 0; i < 500; ++i) {
|
||||
filter.Predict(
|
||||
g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
|
||||
Eigen::Matrix2d::Identity() * 1e-9));
|
||||
filter.Observe<1>(
|
||||
h, GaussianDistribution<double, 1>(Scalar(0.), Scalar(1e-2)));
|
||||
}
|
||||
EXPECT_NEAR(filter.GetBelief().GetMean()[0], 5, 1e-2);
|
||||
EXPECT_NEAR(filter.GetBelief().GetMean()[1], 5, 1e-2);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace kalman_filter
|
||||
} // namespace cartographer
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
|
||||
#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||
#include "cartographer/mapping/submaps.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
// This interface is used for both 2D and 3D SLAM. Implementations wire up a
|
||||
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
|
||||
// to detect loop closure, and a sparse pose graph optimization to compute
|
||||
// optimized pose estimates.
|
||||
class GlobalTrajectoryBuilderInterface {
|
||||
public:
|
||||
// Represents a newly computed pose. Each of the following steps in the pose
|
||||
// estimation pipeline are provided for debugging:
|
||||
//
|
||||
// 1. UKF prediction
|
||||
// 2. Absolute pose observation (e.g. from scan matching)
|
||||
// 3. UKF estimate after integrating any measurements
|
||||
//
|
||||
// Finally, 'pose' is the end-user visualization of orientation and
|
||||
// 'point_cloud' is the point cloud, in the odometry frame, used to make
|
||||
// this estimate.
|
||||
struct PoseEstimate {
|
||||
PoseEstimate() = default;
|
||||
PoseEstimate(common::Time time,
|
||||
const kalman_filter::PoseAndCovariance& prediction,
|
||||
const kalman_filter::PoseAndCovariance& observation,
|
||||
const kalman_filter::PoseAndCovariance& estimate,
|
||||
const transform::Rigid3d& pose,
|
||||
const sensor::PointCloud& point_cloud);
|
||||
|
||||
common::Time time = common::Time::min();
|
||||
kalman_filter::PoseAndCovariance prediction = {
|
||||
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
|
||||
kalman_filter::PoseAndCovariance observation = {
|
||||
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
|
||||
kalman_filter::PoseAndCovariance estimate = {
|
||||
transform::Rigid3d::Identity(), kalman_filter::PoseCovariance::Zero()};
|
||||
transform::Rigid3d pose = transform::Rigid3d::Identity();
|
||||
sensor::PointCloud point_cloud;
|
||||
};
|
||||
|
||||
GlobalTrajectoryBuilderInterface() {}
|
||||
virtual ~GlobalTrajectoryBuilderInterface() {}
|
||||
|
||||
GlobalTrajectoryBuilderInterface(const GlobalTrajectoryBuilderInterface&) =
|
||||
delete;
|
||||
GlobalTrajectoryBuilderInterface& operator=(
|
||||
const GlobalTrajectoryBuilderInterface&) = delete;
|
||||
|
||||
virtual const Submaps* submaps() const = 0;
|
||||
virtual Submaps* submaps() = 0;
|
||||
virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
|
||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||
|
||||
virtual void AddHorizontalLaserFan(common::Time,
|
||||
const sensor::LaserFan3D& laser_fan) = 0;
|
||||
virtual void AddImuData(common::Time time,
|
||||
const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) = 0;
|
||||
virtual void AddLaserFan3D(common::Time time,
|
||||
const sensor::LaserFan3D& laser_fan) = 0;
|
||||
virtual void AddOdometerPose(
|
||||
common::Time time, const transform::Rigid3d& pose,
|
||||
const kalman_filter::PoseCovariance& covariance) = 0;
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
namespace {
|
||||
|
||||
// 0 is unknown, [1, 32767] maps to [kMinProbability, kMaxProbability].
|
||||
float SlowValueToProbability(const uint16 value) {
|
||||
CHECK_GE(value, 0);
|
||||
CHECK_LE(value, 32767);
|
||||
if (value == kUnknownProbabilityValue) {
|
||||
// Unknown cells have kMinProbability.
|
||||
return kMinProbability;
|
||||
}
|
||||
const float kScale = (kMaxProbability - kMinProbability) / 32766.f;
|
||||
return value * kScale + (kMinProbability - kScale);
|
||||
}
|
||||
|
||||
const std::vector<float>* PrecomputeValueToProbability() {
|
||||
std::vector<float>* result = new std::vector<float>;
|
||||
// Repeat two times, so that both values with and without the update marker
|
||||
// can be converted to a probability.
|
||||
for (int repeat = 0; repeat != 2; ++repeat) {
|
||||
for (int value = 0; value != 32768; ++value) {
|
||||
result->push_back(SlowValueToProbability(value));
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
const std::vector<float>* const kValueToProbability =
|
||||
PrecomputeValueToProbability();
|
||||
|
||||
std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
|
||||
std::vector<uint16> result;
|
||||
result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) +
|
||||
kUpdateMarker);
|
||||
for (int cell = 1; cell != 32768; ++cell) {
|
||||
result.push_back(ProbabilityToValue(ProbabilityFromOdds(
|
||||
odds * Odds((*kValueToProbability)[cell]))) +
|
||||
kUpdateMarker);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_
|
||||
#define CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
inline float Odds(float probability) {
|
||||
return probability / (1.f - probability);
|
||||
}
|
||||
|
||||
inline float ProbabilityFromOdds(const float odds) {
|
||||
return odds / (odds + 1.f);
|
||||
}
|
||||
|
||||
constexpr float kMinProbability = 0.1f;
|
||||
constexpr float kMaxProbability = 1.f - kMinProbability;
|
||||
|
||||
// Clamps probability to be in the range [kMinProbability, kMaxProbability].
|
||||
inline float ClampProbability(const float probability) {
|
||||
return common::Clamp(probability, kMinProbability, kMaxProbability);
|
||||
}
|
||||
|
||||
constexpr uint16 kUnknownProbabilityValue = 0;
|
||||
constexpr uint16 kUpdateMarker = 1u << 15;
|
||||
|
||||
// Converts a probability to a uint16 in the [1, 32767] range.
|
||||
inline uint16 ProbabilityToValue(const float probability) {
|
||||
const int value =
|
||||
common::RoundToInt((ClampProbability(probability) - kMinProbability) *
|
||||
(32766.f / (kMaxProbability - kMinProbability))) +
|
||||
1;
|
||||
// DCHECK for performance.
|
||||
DCHECK_GE(value, 1);
|
||||
DCHECK_LE(value, 32767);
|
||||
return value;
|
||||
}
|
||||
|
||||
extern const std::vector<float>* const kValueToProbability;
|
||||
|
||||
// Converts a uint16 (which may or may not have the update marker set) to a
|
||||
// probability in the range [kMinProbability, kMaxProbability].
|
||||
inline float ValueToProbability(const uint16 value) {
|
||||
return (*kValueToProbability)[value];
|
||||
}
|
||||
|
||||
std::vector<uint16> ComputeLookupTableToApplyOdds(float odds);
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_
|
||||
#define CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
#include <utility>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/ordered_multi_queue.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/sensor/sensor_packet_period_histogram_builder.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
struct SensorCollatorQueueKey {
|
||||
int trajectory_id;
|
||||
string sensor_id;
|
||||
|
||||
bool operator<(const SensorCollatorQueueKey& other) const {
|
||||
return std::forward_as_tuple(trajectory_id, sensor_id) <
|
||||
std::forward_as_tuple(other.trajectory_id, other.sensor_id);
|
||||
}
|
||||
};
|
||||
|
||||
inline std::ostream& operator<<(std::ostream& out,
|
||||
const SensorCollatorQueueKey& key) {
|
||||
return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')';
|
||||
}
|
||||
|
||||
template <typename SensorDataType>
|
||||
class SensorCollator {
|
||||
public:
|
||||
using Callback = std::function<void(int64, std::unique_ptr<SensorDataType>)>;
|
||||
|
||||
SensorCollator() {}
|
||||
|
||||
SensorCollator(const SensorCollator&) = delete;
|
||||
SensorCollator& operator=(const SensorCollator&) = delete;
|
||||
|
||||
// Adds a trajectory to produce sorted sensor output for. Calls 'callback'
|
||||
// for each collated sensor data.
|
||||
void AddTrajectory(const int trajectory_id,
|
||||
const std::unordered_set<string>& expected_sensor_ids,
|
||||
const Callback callback) {
|
||||
for (const auto& sensor_id : expected_sensor_ids) {
|
||||
const auto queue_key = SensorCollatorQueueKey{trajectory_id, sensor_id};
|
||||
queue_.AddQueue(queue_key, [callback](std::unique_ptr<Value> value) {
|
||||
callback(value->timestamp, std::move(value->sensor_data));
|
||||
});
|
||||
queue_keys_[trajectory_id].push_back(queue_key);
|
||||
}
|
||||
}
|
||||
|
||||
// Marks 'trajectory_id' as finished.
|
||||
void FinishTrajectory(const int trajectory_id) {
|
||||
for (const auto& queue_key : queue_keys_[trajectory_id]) {
|
||||
queue_.MarkQueueAsFinished(queue_key);
|
||||
}
|
||||
}
|
||||
|
||||
// Adds 'sensor_data' for 'trajectory_id' to be collated. 'sensor_data' must
|
||||
// contain valid sensor data. Sensor packets with matching 'sensor_id' must be
|
||||
// added in time order.
|
||||
void AddSensorData(const int trajectory_id, const int64 timestamp,
|
||||
const string& sensor_id,
|
||||
std::unique_ptr<SensorDataType> sensor_data) {
|
||||
sensor_packet_period_histogram_builder_.Add(trajectory_id, timestamp,
|
||||
sensor_id);
|
||||
queue_.Add(
|
||||
SensorCollatorQueueKey{trajectory_id, sensor_id}, timestamp,
|
||||
common::make_unique<Value>(Value{timestamp, std::move(sensor_data)}));
|
||||
}
|
||||
|
||||
// Dispatches all queued sensor packets. May only be called once.
|
||||
// AddSensorData may not be called after Flush.
|
||||
void Flush() {
|
||||
queue_.Flush();
|
||||
sensor_packet_period_histogram_builder_.LogHistogramsAndClear();
|
||||
}
|
||||
|
||||
// Returns the number of packets associated with 'trajectory_id' that are
|
||||
// available for processing.
|
||||
int num_available_packets(const int trajectory_id) {
|
||||
int num = std::numeric_limits<int>::max();
|
||||
for (const auto& queue_key : queue_keys_[trajectory_id]) {
|
||||
num = std::min(num, queue_.num_available(queue_key));
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
private:
|
||||
struct Value {
|
||||
int64 timestamp;
|
||||
std::unique_ptr<SensorDataType> sensor_data;
|
||||
};
|
||||
|
||||
// Queue keys are a pair of trajectory ID and sensor identifier.
|
||||
common::OrderedMultiQueue<SensorCollatorQueueKey, int64, Value> queue_;
|
||||
|
||||
// Map of trajectory ID to all associated QueueKeys.
|
||||
std::unordered_map<int, std::vector<SensorCollatorQueueKey>> queue_keys_;
|
||||
sensor::SensorPacketPeriodHistogramBuilder
|
||||
sensor_packet_period_histogram_builder_;
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/sensor_collator.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
struct TestData {
|
||||
string frame_id;
|
||||
};
|
||||
|
||||
TEST(SensorCollator, Ordering) {
|
||||
TestData first{"horizontal_laser"};
|
||||
TestData second{"vertical_laser"};
|
||||
TestData third{"imu"};
|
||||
TestData fourth{"horizontal_laser"};
|
||||
TestData fifth{"vertical_laser"};
|
||||
TestData sixth{"something"};
|
||||
|
||||
const std::unordered_set<string> frame_ids = {
|
||||
"horizontal_laser", "vertical_laser", "imu", "something"};
|
||||
std::vector<std::pair<int64, TestData>> received;
|
||||
SensorCollator<TestData> sensor_collator;
|
||||
sensor_collator.AddTrajectory(
|
||||
0, frame_ids,
|
||||
[&received](const int64 timestamp, std::unique_ptr<TestData> packet) {
|
||||
received.push_back(std::make_pair(timestamp, *packet));
|
||||
});
|
||||
|
||||
sensor_collator.AddSensorData(0, 100, first.frame_id,
|
||||
common::make_unique<TestData>(first));
|
||||
sensor_collator.AddSensorData(0, 600, sixth.frame_id,
|
||||
common::make_unique<TestData>(sixth));
|
||||
sensor_collator.AddSensorData(0, 400, fourth.frame_id,
|
||||
common::make_unique<TestData>(fourth));
|
||||
sensor_collator.AddSensorData(0, 200, second.frame_id,
|
||||
common::make_unique<TestData>(second));
|
||||
sensor_collator.AddSensorData(0, 500, fifth.frame_id,
|
||||
common::make_unique<TestData>(fifth));
|
||||
sensor_collator.AddSensorData(0, 300, third.frame_id,
|
||||
common::make_unique<TestData>(third));
|
||||
|
||||
EXPECT_EQ(3, received.size());
|
||||
EXPECT_EQ(100, received[0].first);
|
||||
EXPECT_EQ("horizontal_laser", received[0].second.frame_id);
|
||||
EXPECT_EQ(200, received[1].first);
|
||||
EXPECT_EQ("vertical_laser", received[1].second.frame_id);
|
||||
EXPECT_EQ(300, received[2].first);
|
||||
EXPECT_EQ("imu", received[2].second.frame_id);
|
||||
|
||||
sensor_collator.Flush();
|
||||
|
||||
ASSERT_EQ(6, received.size());
|
||||
EXPECT_EQ("horizontal_laser", received[3].second.frame_id);
|
||||
EXPECT_EQ(500, received[4].first);
|
||||
EXPECT_EQ("vertical_laser", received[4].second.frame_id);
|
||||
EXPECT_EQ(600, received[5].first);
|
||||
EXPECT_EQ("something", received[5].second.frame_id);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/sparse_pose_graph.h"
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
#include "cartographer/mapping/sparse_pose_graph/constraint_builder.h"
|
||||
#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
|
||||
const std::vector<TrajectoryNode>& trajectory_nodes) {
|
||||
std::vector<std::vector<TrajectoryNode>> trajectories;
|
||||
std::unordered_map<const mapping::Submaps*, int> trajectory_ids;
|
||||
for (const auto& node : trajectory_nodes) {
|
||||
const auto* trajectory = node.constant_data->trajectory;
|
||||
if (trajectory_ids.emplace(trajectory, trajectories.size()).second) {
|
||||
trajectories.push_back({node});
|
||||
} else {
|
||||
trajectories[trajectory_ids[trajectory]].push_back(node);
|
||||
}
|
||||
}
|
||||
return trajectories;
|
||||
}
|
||||
|
||||
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
|
||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||
proto::SparsePoseGraphOptions options;
|
||||
options.set_optimize_every_n_scans(
|
||||
parameter_dictionary->GetInt("optimize_every_n_scans"));
|
||||
options.set_also_match_to_new_submaps(
|
||||
parameter_dictionary->GetBool("also_match_to_new_submaps"));
|
||||
*options.mutable_constraint_builder_options() =
|
||||
sparse_pose_graph::CreateConstraintBuilderOptions(
|
||||
parameter_dictionary->GetDictionary("constraint_builder").get());
|
||||
*options.mutable_optimization_problem_options() =
|
||||
mapping::sparse_pose_graph::CreateOptimizationProblemOptions(
|
||||
parameter_dictionary->GetDictionary("optimization_problem").get());
|
||||
options.set_max_num_final_iterations(
|
||||
parameter_dictionary->GetNonNegativeInt("max_num_final_iterations"));
|
||||
CHECK_GT(options.max_num_final_iterations(), 0);
|
||||
options.set_global_sampling_ratio(
|
||||
parameter_dictionary->GetDouble("global_sampling_ratio"));
|
||||
return options;
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,127 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
|
||||
#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/mapping/proto/scan_matching_progress.pb.h"
|
||||
#include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
|
||||
#include "cartographer/mapping/submaps.h"
|
||||
#include "cartographer/mapping/trajectory_node.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
|
||||
common::LuaParameterDictionary* const parameter_dictionary);
|
||||
|
||||
// Splits TrajectoryNodes by ID.
|
||||
std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
|
||||
const std::vector<TrajectoryNode>& trajectory_nodes);
|
||||
|
||||
class SparsePoseGraph {
|
||||
public:
|
||||
// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
|
||||
// pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
|
||||
// 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
|
||||
struct Constraint2D {
|
||||
struct Pose {
|
||||
transform::Rigid2d zbar_ij;
|
||||
Eigen::Matrix<double, 3, 3> sqrt_Lambda_ij;
|
||||
};
|
||||
|
||||
// Submap index.
|
||||
int i;
|
||||
|
||||
// Scan index.
|
||||
int j;
|
||||
|
||||
// Pose of the scan 'j' relative to submap 'i'.
|
||||
Pose pose;
|
||||
|
||||
// Differentiates between intra-submap (where scan 'j' was inserted into
|
||||
// submap 'i') and inter-submap constraints (where scan 'j' was not inserted
|
||||
// into submap 'i').
|
||||
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
|
||||
};
|
||||
|
||||
struct Constraint3D {
|
||||
struct Pose {
|
||||
transform::Rigid3d zbar_ij;
|
||||
Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
|
||||
};
|
||||
|
||||
// Submap index.
|
||||
int i;
|
||||
|
||||
// Scan index.
|
||||
int j;
|
||||
|
||||
// Pose of the scan 'j' relative to submap 'i'.
|
||||
Pose pose;
|
||||
|
||||
// Differentiates between intra-submap (where scan 'j' was inserted into
|
||||
// submap 'i') and inter-submap constraints (where scan 'j' was not inserted
|
||||
// into submap 'i').
|
||||
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
|
||||
};
|
||||
|
||||
SparsePoseGraph() {}
|
||||
virtual ~SparsePoseGraph() {}
|
||||
|
||||
SparsePoseGraph(const SparsePoseGraph&) = delete;
|
||||
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
|
||||
|
||||
// Computes optimized poses.
|
||||
virtual void RunFinalOptimization() = 0;
|
||||
|
||||
// Will once return true whenever new optimized poses are available.
|
||||
virtual bool HasNewOptimizedPoses() = 0;
|
||||
|
||||
// Returns the scan matching progress.
|
||||
virtual proto::ScanMatchingProgress GetScanMatchingProgress() = 0;
|
||||
|
||||
// Get the current trajectory clusters.
|
||||
virtual std::vector<std::vector<const Submaps*>>
|
||||
GetConnectedTrajectories() = 0;
|
||||
|
||||
// Returns the current optimized transforms for the given 'submaps'.
|
||||
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
|
||||
const Submaps& submaps) = 0;
|
||||
|
||||
// Returns the transform converting from odometry based transforms to the
|
||||
// optimized world.
|
||||
virtual transform::Rigid3d GetOdometryToMapTransform(
|
||||
const mapping::Submaps& submaps) = 0;
|
||||
|
||||
// Returns the current optimized trajectory.
|
||||
virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
|
||||
|
||||
// Returns the collection of 2D constraints.
|
||||
virtual std::vector<Constraint2D> constraints_2d() = 0;
|
||||
|
||||
// Returns the collection of 3D constraints.
|
||||
virtual std::vector<Constraint3D> constraints_3d() = 0;
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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
|
||||
|
|
@ -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_
|
||||
|
|
@ -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
|
||||
|
|
@ -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_
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/sparse_pose_graph.h"
|
||||
|
||||
#include <deque>
|
||||
|
||||
#include "glog/logging.h"
|
||||
#include "gmock/gmock.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
class FakeSubmaps : public Submaps {
|
||||
public:
|
||||
const Submap* Get(int) const override { LOG(FATAL) << "Not implemented."; }
|
||||
|
||||
int size() const override { LOG(FATAL) << "Not implemented."; }
|
||||
|
||||
void SubmapToProto(int, const std::vector<mapping::TrajectoryNode>&,
|
||||
const transform::Rigid3d&,
|
||||
proto::SubmapQuery::Response*) override {
|
||||
LOG(FATAL) << "Not implemented.";
|
||||
}
|
||||
};
|
||||
|
||||
TEST(SparsePoseGraphTest, SplitTrajectoryNodes) {
|
||||
std::vector<TrajectoryNode> trajectory_nodes;
|
||||
const std::vector<FakeSubmaps> submaps(5);
|
||||
std::deque<TrajectoryNode::ConstantData> constant_data;
|
||||
constexpr int kNumTrajectoryNodes = 10;
|
||||
for (int j = 0; j < kNumTrajectoryNodes; ++j) {
|
||||
for (size_t i = 0; i < submaps.size(); ++i) {
|
||||
constant_data.push_back({});
|
||||
constant_data.back().trajectory = &submaps[i];
|
||||
TrajectoryNode node;
|
||||
node.constant_data = &constant_data.back();
|
||||
trajectory_nodes.push_back(node);
|
||||
}
|
||||
}
|
||||
std::vector<std::vector<TrajectoryNode>> split_trajectory_nodes =
|
||||
SplitTrajectoryNodes(trajectory_nodes);
|
||||
EXPECT_EQ(split_trajectory_nodes.size(), submaps.size());
|
||||
for (size_t i = 0; i < submaps.size(); ++i) {
|
||||
EXPECT_EQ(split_trajectory_nodes[i].size(), kNumTrajectoryNodes);
|
||||
for (const auto& node : split_trajectory_nodes[i]) {
|
||||
EXPECT_EQ(node.constant_data->trajectory, &submaps[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/submaps.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
constexpr uint8 Submaps::kUnknownLogOdds;
|
||||
|
||||
Submaps::Submaps() {}
|
||||
|
||||
Submaps::~Submaps() {}
|
||||
|
||||
int Submaps::matching_index() const {
|
||||
if (size() > 1) {
|
||||
return size() - 2;
|
||||
}
|
||||
return size() - 1;
|
||||
}
|
||||
|
||||
std::vector<int> Submaps::insertion_indices() const {
|
||||
if (size() > 1) {
|
||||
return {size() - 2, size() - 1};
|
||||
}
|
||||
return {size() - 1};
|
||||
}
|
||||
|
||||
void Submaps::AddProbabilityGridToResponse(
|
||||
const transform::Rigid3d& local_submap_pose,
|
||||
const mapping_2d::ProbabilityGrid& probability_grid,
|
||||
proto::SubmapQuery::Response* response) {
|
||||
Eigen::Array2i offset;
|
||||
mapping_2d::CellLimits limits;
|
||||
probability_grid.ComputeCroppedLimits(&offset, &limits);
|
||||
|
||||
string cells;
|
||||
for (const Eigen::Array2i& xy_index :
|
||||
mapping_2d::XYIndexRangeIterator(limits)) {
|
||||
if (probability_grid.IsKnown(xy_index + offset)) {
|
||||
// We would like to add 'delta' but this is not possible using a value and
|
||||
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
|
||||
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
|
||||
// zero, and use 'alpha' to subtract. This is only correct when the pixel
|
||||
// is currently white, so walls will look too gray. This should be hard to
|
||||
// detect visually for the user, though.
|
||||
const int delta =
|
||||
128 - ProbabilityToLogOddsInteger(
|
||||
probability_grid.GetProbability(xy_index + offset));
|
||||
const uint8 alpha = delta > 0 ? 0 : -delta;
|
||||
const uint8 value = delta > 0 ? delta : 0;
|
||||
cells.push_back(value);
|
||||
cells.push_back((value || alpha) ? alpha : 1);
|
||||
} else {
|
||||
cells.push_back(static_cast<uint8>(kUnknownLogOdds)); // value
|
||||
cells.push_back(0); // alpha
|
||||
}
|
||||
}
|
||||
common::FastGzipString(cells, response->mutable_cells());
|
||||
|
||||
response->set_width(limits.num_x_cells);
|
||||
response->set_height(limits.num_y_cells);
|
||||
const double resolution = probability_grid.limits().resolution();
|
||||
response->set_resolution(resolution);
|
||||
const double max_x = probability_grid.limits().edge_limits().max().x() -
|
||||
resolution * offset.y();
|
||||
const double max_y = probability_grid.limits().edge_limits().max().y() -
|
||||
resolution * offset.x();
|
||||
*response->mutable_slice_pose() = transform::ToProto(
|
||||
local_submap_pose.inverse() *
|
||||
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,136 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
// Submaps is a sequence of maps to which scans are matched and into which scans
|
||||
// are inserted.
|
||||
//
|
||||
// Except during initialization when only a single submap exists, there are
|
||||
// always two submaps into which scans are inserted: an old submap that is used
|
||||
// for matching, and a new one, which will be used for matching next, that is
|
||||
// being initialized.
|
||||
//
|
||||
// Once a certain number of scans have been inserted, the new submap is
|
||||
// considered initialized: the old submap is no longer changed, the "new" submap
|
||||
// is now the "old" submap and is used for scan-to-map matching. Moreover,
|
||||
// a "new" submap gets inserted.
|
||||
#ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_
|
||||
#define CARTOGRAPHER_MAPPING_SUBMAPS_H_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping/proto/submaps.pb.h"
|
||||
#include "cartographer/mapping/trajectory_node.h"
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
// Converts the given probability to log odds.
|
||||
inline float Logit(float probability) {
|
||||
return std::log(probability / (1.f - probability));
|
||||
}
|
||||
|
||||
const float kMaxLogOdds = Logit(kMaxProbability);
|
||||
const float kMinLogOdds = Logit(kMinProbability);
|
||||
|
||||
// Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
|
||||
// kMaxLogOdds] is mapped to [1, 255].
|
||||
inline uint8 ProbabilityToLogOddsInteger(const float probability) {
|
||||
const int value = common::RoundToInt((Logit(probability) - kMinLogOdds) *
|
||||
254.f / (kMaxLogOdds - kMinLogOdds)) +
|
||||
1;
|
||||
CHECK_LE(1, value);
|
||||
CHECK_GE(255, value);
|
||||
return value;
|
||||
}
|
||||
|
||||
// An individual submap, which has an initial position 'origin', keeps track of
|
||||
// which laser fans where inserted into it, and sets the
|
||||
// 'finished_probability_grid' to be used for loop closing once the map no
|
||||
// longer changes.
|
||||
struct Submap {
|
||||
Submap(const Eigen::Vector3f& origin, int begin_laser_fan_index)
|
||||
: origin(origin),
|
||||
begin_laser_fan_index(begin_laser_fan_index),
|
||||
end_laser_fan_index(begin_laser_fan_index) {}
|
||||
|
||||
transform::Rigid3d local_pose() const {
|
||||
return transform::Rigid3d::Translation(origin.cast<double>());
|
||||
}
|
||||
|
||||
// Origin of this submap.
|
||||
Eigen::Vector3f origin;
|
||||
|
||||
// This Submap contains LaserFans with indices in the range
|
||||
// ['begin_laser_fan_index', 'end_laser_fan_index').
|
||||
int begin_laser_fan_index;
|
||||
int end_laser_fan_index;
|
||||
|
||||
// The 'finished_probability_grid' when this submap is finished and will not
|
||||
// change anymore. Otherwise, this is nullptr and the next call to
|
||||
// InsertLaserFan() will change the submap.
|
||||
const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
|
||||
};
|
||||
|
||||
// A container of Submaps.
|
||||
class Submaps {
|
||||
public:
|
||||
static constexpr uint8 kUnknownLogOdds = 0;
|
||||
|
||||
Submaps();
|
||||
virtual ~Submaps();
|
||||
|
||||
Submaps(const Submaps&) = delete;
|
||||
Submaps& operator=(const Submaps&) = delete;
|
||||
|
||||
// Returns the index of the newest initialized Submap which can be
|
||||
// used for scan-to-map matching.
|
||||
int matching_index() const;
|
||||
|
||||
// Returns the indices of the Submap into which point clouds will
|
||||
// be inserted.
|
||||
std::vector<int> insertion_indices() const;
|
||||
|
||||
// Returns the Submap with the given 'index'. The same 'index' will always
|
||||
// return the same pointer, so that Submaps can be identified by it.
|
||||
virtual const Submap* Get(int index) const = 0;
|
||||
|
||||
// Returns the number of Submaps.
|
||||
virtual int size() const = 0;
|
||||
|
||||
// Fills data about the Submap with 'index' into the 'response'.
|
||||
virtual void SubmapToProto(
|
||||
int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
|
||||
const transform::Rigid3d& global_submap_pose,
|
||||
proto::SubmapQuery::Response* response) = 0;
|
||||
|
||||
protected:
|
||||
static void AddProbabilityGridToResponse(
|
||||
const transform::Rigid3d& local_submap_pose,
|
||||
const mapping_2d::ProbabilityGrid& probability_grid,
|
||||
proto::SubmapQuery::Response* response);
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_SUBMAPS_H_
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/submaps.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
// Converts the given log odds to a probability. This function is known to be
|
||||
// very slow, because expf is incredibly slow.
|
||||
inline float Expit(float log_odds) {
|
||||
const float exp_log_odds = std::exp(log_odds);
|
||||
return exp_log_odds / (1.f + exp_log_odds);
|
||||
}
|
||||
|
||||
TEST(SubmapsTest, LogOddsConversions) {
|
||||
EXPECT_NEAR(Expit(Logit(kMinProbability)), kMinProbability, 1e-6);
|
||||
EXPECT_NEAR(Expit(Logit(kMaxProbability)), kMaxProbability, 1e-6);
|
||||
EXPECT_NEAR(Expit(Logit(0.5)), 0.5, 1e-6);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,149 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/trajectory_connectivity.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <unordered_set>
|
||||
|
||||
#include "cartographer/mapping/proto/trajectory_connectivity.pb.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
TrajectoryConnectivity::TrajectoryConnectivity()
|
||||
: lock_(), forest_(), connection_map_() {}
|
||||
|
||||
void TrajectoryConnectivity::Add(const Submaps* trajectory) {
|
||||
CHECK(trajectory != nullptr);
|
||||
common::MutexLocker locker(&lock_);
|
||||
forest_.emplace(trajectory, trajectory);
|
||||
}
|
||||
|
||||
void TrajectoryConnectivity::Connect(const Submaps* trajectory_a,
|
||||
const Submaps* trajectory_b) {
|
||||
CHECK(trajectory_a != nullptr);
|
||||
CHECK(trajectory_b != nullptr);
|
||||
common::MutexLocker locker(&lock_);
|
||||
Union(trajectory_a, trajectory_b);
|
||||
auto sorted_pair =
|
||||
std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>());
|
||||
++connection_map_[sorted_pair];
|
||||
}
|
||||
|
||||
void TrajectoryConnectivity::Union(const Submaps* const trajectory_a,
|
||||
const Submaps* const trajectory_b) {
|
||||
forest_.emplace(trajectory_a, trajectory_a);
|
||||
forest_.emplace(trajectory_b, trajectory_b);
|
||||
const Submaps* const representative_a = FindSet(trajectory_a);
|
||||
const Submaps* const representative_b = FindSet(trajectory_b);
|
||||
forest_[representative_a] = representative_b;
|
||||
}
|
||||
|
||||
const Submaps* TrajectoryConnectivity::FindSet(
|
||||
const Submaps* const trajectory) {
|
||||
auto it = forest_.find(trajectory);
|
||||
CHECK(it != forest_.end());
|
||||
if (it->first != it->second) {
|
||||
it->second = FindSet(it->second);
|
||||
}
|
||||
return it->second;
|
||||
}
|
||||
|
||||
bool TrajectoryConnectivity::TransitivelyConnected(
|
||||
const Submaps* trajectory_a, const Submaps* trajectory_b) {
|
||||
CHECK(trajectory_a != nullptr);
|
||||
CHECK(trajectory_b != nullptr);
|
||||
common::MutexLocker locker(&lock_);
|
||||
|
||||
if (forest_.count(trajectory_a) == 0 || forest_.count(trajectory_b) == 0) {
|
||||
return false;
|
||||
}
|
||||
return FindSet(trajectory_a) == FindSet(trajectory_b);
|
||||
}
|
||||
|
||||
std::vector<std::vector<const Submaps*>>
|
||||
TrajectoryConnectivity::ConnectedComponents() {
|
||||
// Map from cluster exemplar -> growing cluster.
|
||||
std::unordered_map<const Submaps*, std::vector<const Submaps*>> map;
|
||||
common::MutexLocker locker(&lock_);
|
||||
for (const auto& trajectory_entry : forest_) {
|
||||
map[FindSet(trajectory_entry.first)].push_back(trajectory_entry.first);
|
||||
}
|
||||
|
||||
std::vector<std::vector<const Submaps*>> result;
|
||||
result.reserve(map.size());
|
||||
for (auto& pair : map) {
|
||||
result.emplace_back(std::move(pair.second));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int TrajectoryConnectivity::ConnectionCount(const Submaps* trajectory_a,
|
||||
const Submaps* trajectory_b) {
|
||||
CHECK(trajectory_a != nullptr);
|
||||
CHECK(trajectory_b != nullptr);
|
||||
common::MutexLocker locker(&lock_);
|
||||
const auto it = connection_map_.find(
|
||||
std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>()));
|
||||
return it != connection_map_.end() ? it->second : 0;
|
||||
}
|
||||
|
||||
proto::TrajectoryConnectivity ToProto(
|
||||
std::vector<std::vector<const Submaps*>> connected_components,
|
||||
std::unordered_map<const mapping::Submaps*, int> trajectory_indices) {
|
||||
proto::TrajectoryConnectivity proto;
|
||||
std::vector<std::vector<int>> connected_components_by_indices;
|
||||
for (const auto& connected_component : connected_components) {
|
||||
connected_components_by_indices.emplace_back();
|
||||
for (const mapping::Submaps* trajectory : connected_component) {
|
||||
connected_components_by_indices.back().push_back(
|
||||
trajectory_indices.at(trajectory));
|
||||
}
|
||||
std::sort(connected_components_by_indices.back().begin(),
|
||||
connected_components_by_indices.back().end());
|
||||
}
|
||||
std::sort(connected_components_by_indices.begin(),
|
||||
connected_components_by_indices.end());
|
||||
for (const auto& connected_component : connected_components_by_indices) {
|
||||
auto* proto_connected_component = proto.add_connected_component();
|
||||
for (const int trajectory_id : connected_component) {
|
||||
proto_connected_component->add_trajectory_id(trajectory_id);
|
||||
}
|
||||
}
|
||||
return proto;
|
||||
}
|
||||
|
||||
proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
|
||||
const proto::TrajectoryConnectivity& trajectory_connectivity,
|
||||
const int trajectory_id) {
|
||||
for (const auto& connected_component :
|
||||
trajectory_connectivity.connected_component()) {
|
||||
if (std::find(connected_component.trajectory_id().begin(),
|
||||
connected_component.trajectory_id().end(),
|
||||
trajectory_id) != connected_component.trajectory_id().end()) {
|
||||
return connected_component;
|
||||
}
|
||||
}
|
||||
|
||||
proto::TrajectoryConnectivity::ConnectedComponent connected_component;
|
||||
connected_component.add_trajectory_id(trajectory_id);
|
||||
return connected_component;
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_
|
||||
#define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_
|
||||
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "cartographer/common/mutex.h"
|
||||
#include "cartographer/mapping/proto/trajectory_connectivity.pb.h"
|
||||
#include "cartographer/mapping/submaps.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
// A class that tracks the connectivity structure between trajectories.
|
||||
//
|
||||
// Connectivity includes both the count ("How many times have I _directly_
|
||||
// connected trajectories i and j?") and the transitive connectivity.
|
||||
//
|
||||
// This class is thread-safe.
|
||||
class TrajectoryConnectivity {
|
||||
public:
|
||||
TrajectoryConnectivity();
|
||||
|
||||
TrajectoryConnectivity(const TrajectoryConnectivity&) = delete;
|
||||
TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete;
|
||||
|
||||
// Add a trajectory which is initially connected to nothing.
|
||||
void Add(const Submaps* trajectory) EXCLUDES(lock_);
|
||||
|
||||
// Connect two trajectories. If either trajectory is untracked, it will be
|
||||
// tracked. This function is invariant to the order of its arguments. Repeated
|
||||
// calls to Connect increment the connectivity count.
|
||||
void Connect(const Submaps* trajectory_a, const Submaps* trajectory_b)
|
||||
EXCLUDES(lock_);
|
||||
|
||||
// Determines if two trajectories have been (transitively) connected. If
|
||||
// either trajectory is not being tracked, returns false. This function is
|
||||
// invariant to the order of its arguments.
|
||||
bool TransitivelyConnected(const Submaps* trajectory_a,
|
||||
const Submaps* trajectory_b) EXCLUDES(lock_);
|
||||
|
||||
// Return the number of _direct_ connections between trajectory_a and
|
||||
// trajectory_b. If either trajectory is not being tracked, returns 0. This
|
||||
// function is invariant to the order of its arguments.
|
||||
int ConnectionCount(const Submaps* trajectory_a, const Submaps* trajectory_b)
|
||||
EXCLUDES(lock_);
|
||||
|
||||
// The trajectories, grouped by connectivity.
|
||||
std::vector<std::vector<const Submaps*>> ConnectedComponents()
|
||||
EXCLUDES(lock_);
|
||||
|
||||
private:
|
||||
// Find the representative and compresses the path to it.
|
||||
const Submaps* FindSet(const Submaps* trajectory) REQUIRES(lock_);
|
||||
void Union(const Submaps* trajectory_a, const Submaps* trajectory_b)
|
||||
REQUIRES(lock_);
|
||||
|
||||
common::Mutex lock_;
|
||||
// Tracks transitive connectivity using a disjoint set forest, i.e. each
|
||||
// entry points towards the representative for the given trajectory.
|
||||
std::map<const Submaps*, const Submaps*> forest_ GUARDED_BY(lock_);
|
||||
// Tracks the number of direct connections between a pair of trajectories.
|
||||
std::map<std::pair<const Submaps*, const Submaps*>, int> connection_map_
|
||||
GUARDED_BY(lock_);
|
||||
};
|
||||
|
||||
// Returns a proto encoding connected components according to
|
||||
// 'trajectory_indices'.
|
||||
proto::TrajectoryConnectivity ToProto(
|
||||
std::vector<std::vector<const Submaps*>> connected_components,
|
||||
std::unordered_map<const mapping::Submaps*, int> trajectory_indices);
|
||||
|
||||
// Returns the connected component containing 'trajectory_index'.
|
||||
proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
|
||||
const cartographer::mapping::proto::TrajectoryConnectivity&
|
||||
trajectory_connectivity,
|
||||
int trajectory_id);
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/trajectory_connectivity.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/mapping_2d/submaps.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
class TrajectoryConnectivityTest : public ::testing::Test {
|
||||
protected:
|
||||
TrajectoryConnectivityTest() {
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
auto parameter_dictionary = common::MakeDictionary(R"text(
|
||||
return {
|
||||
resolution = 0.05,
|
||||
half_length = 10.,
|
||||
num_laser_fans = 10,
|
||||
output_debug_images = false,
|
||||
laser_fan_inserter = {
|
||||
insert_free_space = true,
|
||||
hit_probability = 0.53,
|
||||
miss_probability = 0.495,
|
||||
},
|
||||
})text");
|
||||
trajectories_.emplace_back(new mapping_2d::Submaps(
|
||||
mapping_2d::CreateSubmapsOptions(parameter_dictionary.get())));
|
||||
}
|
||||
}
|
||||
|
||||
// Helper function to avoid .get() noise.
|
||||
const Submaps* trajectory(int index) { return trajectories_.at(index).get(); }
|
||||
|
||||
TrajectoryConnectivity trajectory_connectivity_;
|
||||
std::vector<std::unique_ptr<const Submaps>> trajectories_;
|
||||
};
|
||||
|
||||
TEST_F(TrajectoryConnectivityTest, TransitivelyConnected) {
|
||||
// Make sure nothing's connected until we connect some things.
|
||||
for (auto& trajectory_a : trajectories_) {
|
||||
for (auto& trajectory_b : trajectories_) {
|
||||
EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(
|
||||
trajectory_a.get(), trajectory_b.get()));
|
||||
}
|
||||
}
|
||||
|
||||
// Connect some stuff up.
|
||||
trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
|
||||
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
|
||||
trajectory(1)));
|
||||
trajectory_connectivity_.Connect(trajectory(8), trajectory(9));
|
||||
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(8),
|
||||
trajectory(9)));
|
||||
EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
|
||||
trajectory(9)));
|
||||
|
||||
trajectory_connectivity_.Connect(trajectory(1), trajectory(8));
|
||||
for (int i : {0, 1}) {
|
||||
for (int j : {8, 9}) {
|
||||
EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(
|
||||
trajectory(i), trajectory(j)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(TrajectoryConnectivityTest, EmptyConnectedComponents) {
|
||||
auto connections = trajectory_connectivity_.ConnectedComponents();
|
||||
EXPECT_EQ(0, connections.size());
|
||||
}
|
||||
|
||||
TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
|
||||
for (int i = 0; i <= 4; ++i) {
|
||||
trajectory_connectivity_.Connect(trajectory(0), trajectory(i));
|
||||
}
|
||||
for (int i = 5; i <= 9; ++i) {
|
||||
trajectory_connectivity_.Connect(trajectory(5), trajectory(i));
|
||||
}
|
||||
auto connections = trajectory_connectivity_.ConnectedComponents();
|
||||
ASSERT_EQ(2, connections.size());
|
||||
// The clustering is arbitrary; we need to figure out which one is which.
|
||||
const std::vector<const Submaps*>* zero_cluster = nullptr;
|
||||
const std::vector<const Submaps*>* five_cluster = nullptr;
|
||||
if (std::find(connections[0].begin(), connections[0].end(), trajectory(0)) !=
|
||||
connections[0].end()) {
|
||||
zero_cluster = &connections[0];
|
||||
five_cluster = &connections[1];
|
||||
} else {
|
||||
zero_cluster = &connections[1];
|
||||
five_cluster = &connections[0];
|
||||
}
|
||||
for (int i = 0; i <= 9; ++i) {
|
||||
EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(),
|
||||
trajectory(i)) != zero_cluster->end());
|
||||
EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(),
|
||||
trajectory(i)) != five_cluster->end());
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(TrajectoryConnectivityTest, ConnectionCount) {
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
|
||||
// Permute the arguments to check invariance.
|
||||
EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1),
|
||||
trajectory(0)));
|
||||
}
|
||||
for (int i = 1; i < 9; ++i) {
|
||||
EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
|
||||
trajectory(i + 1)));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
|
||||
#define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
|
||||
|
||||
#include <deque>
|
||||
#include <vector>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
struct Submaps;
|
||||
|
||||
struct TrajectoryNode {
|
||||
struct ConstantData {
|
||||
common::Time time;
|
||||
|
||||
// LaserFan in 'pose' frame. Only used in the 2D case.
|
||||
sensor::LaserFan laser_fan;
|
||||
|
||||
// LaserFan in 'pose' frame. Only used in the 3D case.
|
||||
sensor::CompressedLaserFan3D laser_fan_3d;
|
||||
|
||||
// Trajectory this node belongs to.
|
||||
// TODO(jmason): The naming here is confusing because 'trajectory' doesn't
|
||||
// seem like a good name for a Submaps*. Sort this out.
|
||||
const Submaps* trajectory;
|
||||
|
||||
// Transform from the 3D 'tracking' frame to the 'pose' frame of the
|
||||
// laser, which contains roll, pitch and height for 2D. In 3D this is
|
||||
// always identity.
|
||||
transform::Rigid3d tracking_to_pose;
|
||||
};
|
||||
|
||||
common::Time time() const { return constant_data->time; }
|
||||
|
||||
const ConstantData* constant_data;
|
||||
|
||||
transform::Rigid3d pose;
|
||||
};
|
||||
|
||||
// Users will only be interested in 'trajectory_nodes'. But 'constant_data'
|
||||
// is referenced by 'trajectory_nodes'. This struct guarantees that their
|
||||
// lifetimes are bound.
|
||||
struct TrajectoryNodes {
|
||||
std::deque<mapping::TrajectoryNode::ConstantData> constant_data;
|
||||
std::vector<mapping::TrajectoryNode> trajectory_nodes;
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_2d/global_trajectory_builder.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options,
|
||||
SparsePoseGraph* sparse_pose_graph)
|
||||
: options_(options),
|
||||
sparse_pose_graph_(sparse_pose_graph),
|
||||
local_trajectory_builder_(options) {}
|
||||
|
||||
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
||||
|
||||
const Submaps* GlobalTrajectoryBuilder::submaps() const {
|
||||
return local_trajectory_builder_.submaps();
|
||||
}
|
||||
|
||||
Submaps* GlobalTrajectoryBuilder::submaps() {
|
||||
return local_trajectory_builder_.submaps();
|
||||
}
|
||||
|
||||
kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
|
||||
return local_trajectory_builder_.pose_tracker();
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
|
||||
const common::Time time, const sensor::LaserFan3D& laser_fan) {
|
||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
|
||||
local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
|
||||
if (insertion_result != nullptr) {
|
||||
sparse_pose_graph_->AddScan(
|
||||
insertion_result->time, insertion_result->tracking_to_tracking_2d,
|
||||
insertion_result->laser_fan_in_tracking_2d,
|
||||
insertion_result->pose_estimate_2d,
|
||||
kalman_filter::Project2D(insertion_result->covariance_estimate),
|
||||
insertion_result->submaps, insertion_result->matching_submap,
|
||||
insertion_result->insertion_submaps);
|
||||
}
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddImuData(
|
||||
const common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) {
|
||||
local_trajectory_builder_.AddImuData(time, linear_acceleration,
|
||||
angular_velocity);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddOdometerPose(
|
||||
const common::Time time, const transform::Rigid3d& pose,
|
||||
const kalman_filter::PoseCovariance& covariance) {
|
||||
local_trajectory_builder_.AddOdometerPose(time, pose, covariance);
|
||||
}
|
||||
|
||||
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&
|
||||
GlobalTrajectoryBuilder::pose_estimate() const {
|
||||
return local_trajectory_builder_.pose_estimate();
|
||||
}
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
|
@ -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_
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_2d/laser_fan_inserter.h"
|
||||
|
||||
#include <cstdlib>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/mapping_2d/ray_casting.h"
|
||||
#include "cartographer/mapping_2d/xy_index.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
proto::LaserFanInserterOptions CreateLaserFanInserterOptions(
|
||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||
proto::LaserFanInserterOptions options;
|
||||
options.set_hit_probability(
|
||||
parameter_dictionary->GetDouble("hit_probability"));
|
||||
options.set_miss_probability(
|
||||
parameter_dictionary->GetDouble("miss_probability"));
|
||||
options.set_insert_free_space(
|
||||
parameter_dictionary->HasKey("insert_free_space")
|
||||
? parameter_dictionary->GetBool("insert_free_space")
|
||||
: true);
|
||||
CHECK_GT(options.hit_probability(), 0.5);
|
||||
CHECK_LT(options.miss_probability(), 0.5);
|
||||
return options;
|
||||
}
|
||||
|
||||
LaserFanInserter::LaserFanInserter(
|
||||
const proto::LaserFanInserterOptions& options)
|
||||
: options_(options),
|
||||
hit_table_(mapping::ComputeLookupTableToApplyOdds(
|
||||
mapping::Odds(options.hit_probability()))),
|
||||
miss_table_(mapping::ComputeLookupTableToApplyOdds(
|
||||
mapping::Odds(options.miss_probability()))) {}
|
||||
|
||||
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,
|
||||
ProbabilityGrid* const probability_grid) const {
|
||||
CHECK_NOTNULL(probability_grid)->StartUpdate();
|
||||
|
||||
// By not starting a new update after hits are inserted, we give hits priority
|
||||
// (i.e. no hits will be ignored because of a miss in the same cell).
|
||||
CastRays(laser_fan, probability_grid->limits(),
|
||||
[this, &probability_grid](const Eigen::Array2i& hit) {
|
||||
probability_grid->ApplyLookupTable(hit, hit_table_);
|
||||
},
|
||||
[this, &probability_grid](const Eigen::Array2i& miss) {
|
||||
if (options_.insert_free_space()) {
|
||||
probability_grid->ApplyLookupTable(miss, miss_table_);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_
|
||||
#define CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
#include "cartographer/mapping_2d/proto/laser_fan_inserter_options.pb.h"
|
||||
#include "cartographer/mapping_2d/xy_index.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
proto::LaserFanInserterOptions CreateLaserFanInserterOptions(
|
||||
common::LuaParameterDictionary* parameter_dictionary);
|
||||
|
||||
class LaserFanInserter {
|
||||
public:
|
||||
explicit LaserFanInserter(const proto::LaserFanInserterOptions& options);
|
||||
|
||||
LaserFanInserter(const LaserFanInserter&) = delete;
|
||||
LaserFanInserter& operator=(const LaserFanInserter&) = delete;
|
||||
|
||||
// Inserts 'laser_fan' into 'probability_grid'.
|
||||
void Insert(const sensor::LaserFan& laser_fan,
|
||||
ProbabilityGrid* probability_grid) const;
|
||||
|
||||
const std::vector<uint16>& hit_table() const { return hit_table_; }
|
||||
const std::vector<uint16>& miss_table() const { return miss_table_; }
|
||||
|
||||
private:
|
||||
const proto::LaserFanInserterOptions options_;
|
||||
const std::vector<uint16> hit_table_;
|
||||
const std::vector<uint16> miss_table_;
|
||||
};
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_
|
||||
|
|
@ -0,0 +1,130 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_2d/laser_fan_inserter.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
#include "gmock/gmock.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
namespace {
|
||||
|
||||
class LaserFanInserterTest : public ::testing::Test {
|
||||
protected:
|
||||
LaserFanInserterTest()
|
||||
: probability_grid_(
|
||||
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.),
|
||||
Eigen::Vector2d(0., 4.)))) {
|
||||
const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
|
||||
CHECK_EQ(5, cell_limits.num_x_cells);
|
||||
CHECK_EQ(5, cell_limits.num_y_cells);
|
||||
|
||||
auto parameter_dictionary = common::MakeDictionary(
|
||||
"return { "
|
||||
"insert_free_space = true, "
|
||||
"hit_probability = 0.7, "
|
||||
"miss_probability = 0.4, "
|
||||
"}");
|
||||
options_ = CreateLaserFanInserterOptions(parameter_dictionary.get());
|
||||
laser_fan_inserter_ = common::make_unique<LaserFanInserter>(options_);
|
||||
}
|
||||
|
||||
void InsertPointCloud() {
|
||||
sensor::LaserFan laser_fan;
|
||||
laser_fan.point_cloud.emplace_back(-3.5, 0.5);
|
||||
laser_fan.point_cloud.emplace_back(-2.5, 1.5);
|
||||
laser_fan.point_cloud.emplace_back(-1.5, 2.5);
|
||||
laser_fan.point_cloud.emplace_back(-0.5, 3.5);
|
||||
laser_fan.origin.x() = -0.5;
|
||||
laser_fan.origin.y() = 0.5;
|
||||
probability_grid_.StartUpdate();
|
||||
laser_fan_inserter_->Insert(laser_fan, &probability_grid_);
|
||||
}
|
||||
|
||||
ProbabilityGrid probability_grid_;
|
||||
std::unique_ptr<LaserFanInserter> laser_fan_inserter_;
|
||||
proto::LaserFanInserterOptions options_;
|
||||
};
|
||||
|
||||
TEST_F(LaserFanInserterTest, InsertPointCloud) {
|
||||
InsertPointCloud();
|
||||
|
||||
const Eigen::AlignedBox2d& edge_limits =
|
||||
probability_grid_.limits().edge_limits();
|
||||
EXPECT_NEAR(-4., edge_limits.min().x(), 1e-9);
|
||||
EXPECT_NEAR(0., edge_limits.min().y(), 1e-9);
|
||||
EXPECT_NEAR(1., edge_limits.max().x(), 1e-9);
|
||||
EXPECT_NEAR(5., edge_limits.max().y(), 1e-9);
|
||||
|
||||
const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
|
||||
EXPECT_EQ(5, cell_limits.num_x_cells);
|
||||
EXPECT_EQ(5, cell_limits.num_y_cells);
|
||||
|
||||
enum class State { UNKNOWN, MISS, HIT };
|
||||
State expected_states[5][5] = {
|
||||
{State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN,
|
||||
State::UNKNOWN},
|
||||
{State::UNKNOWN, State::HIT, State::MISS, State::MISS, State::MISS},
|
||||
{State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS, State::MISS},
|
||||
{State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS},
|
||||
{State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN,
|
||||
State::HIT}};
|
||||
for (int row = 0; row != 5; ++row) {
|
||||
for (int column = 0; column != 5; ++column) {
|
||||
Eigen::Array2i xy_index(row, column);
|
||||
EXPECT_TRUE(probability_grid_.limits().Contains(xy_index));
|
||||
switch (expected_states[column][row]) {
|
||||
case State::UNKNOWN:
|
||||
EXPECT_FALSE(probability_grid_.IsKnown(xy_index));
|
||||
break;
|
||||
case State::MISS:
|
||||
EXPECT_NEAR(options_.miss_probability(),
|
||||
probability_grid_.GetProbability(xy_index), 1e-4);
|
||||
break;
|
||||
case State::HIT:
|
||||
EXPECT_NEAR(options_.hit_probability(),
|
||||
probability_grid_.GetProbability(xy_index), 1e-4);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(LaserFanInserterTest, ProbabilityProgression) {
|
||||
InsertPointCloud();
|
||||
EXPECT_NEAR(options_.hit_probability(),
|
||||
probability_grid_.GetProbability(-3.5, 0.5), 1e-4);
|
||||
EXPECT_NEAR(options_.miss_probability(),
|
||||
probability_grid_.GetProbability(-2.5, 0.5), 1e-4);
|
||||
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
InsertPointCloud();
|
||||
}
|
||||
EXPECT_NEAR(mapping::kMaxProbability,
|
||||
probability_grid_.GetProbability(-3.5, 0.5), 1e-3);
|
||||
EXPECT_NEAR(mapping::kMinProbability,
|
||||
probability_grid_.GetProbability(-2.5, 0.5), 1e-3);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,284 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_2d/local_trajectory_builder.h"
|
||||
|
||||
#include <limits>
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
|
||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||
proto::LocalTrajectoryBuilderOptions options;
|
||||
options.set_horizontal_laser_min_z(
|
||||
parameter_dictionary->GetDouble("horizontal_laser_min_z"));
|
||||
options.set_horizontal_laser_max_z(
|
||||
parameter_dictionary->GetDouble("horizontal_laser_max_z"));
|
||||
options.set_horizontal_laser_voxel_filter_size(
|
||||
parameter_dictionary->GetDouble("horizontal_laser_voxel_filter_size"));
|
||||
options.set_use_online_correlative_scan_matching(
|
||||
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
|
||||
*options.mutable_adaptive_voxel_filter_options() =
|
||||
sensor::CreateAdaptiveVoxelFilterOptions(
|
||||
parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
|
||||
*options.mutable_real_time_correlative_scan_matcher_options() =
|
||||
scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
|
||||
parameter_dictionary
|
||||
->GetDictionary("real_time_correlative_scan_matcher")
|
||||
.get());
|
||||
*options.mutable_ceres_scan_matcher_options() =
|
||||
scan_matching::CreateCeresScanMatcherOptions(
|
||||
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
|
||||
*options.mutable_motion_filter_options() =
|
||||
mapping_3d::CreateMotionFilterOptions(
|
||||
parameter_dictionary->GetDictionary("motion_filter").get());
|
||||
*options.mutable_pose_tracker_options() =
|
||||
kalman_filter::CreatePoseTrackerOptions(
|
||||
parameter_dictionary->GetDictionary("pose_tracker").get());
|
||||
*options.mutable_submaps_options() = CreateSubmapsOptions(
|
||||
parameter_dictionary->GetDictionary("submaps").get());
|
||||
options.set_expect_imu_data(parameter_dictionary->GetBool("expect_imu_data"));
|
||||
return options;
|
||||
}
|
||||
|
||||
LocalTrajectoryBuilder::LocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options)
|
||||
: options_(options),
|
||||
submaps_(options.submaps_options()),
|
||||
scan_matcher_pose_estimate_(transform::Rigid3d::Identity()),
|
||||
motion_filter_(options_.motion_filter_options()),
|
||||
real_time_correlative_scan_matcher_(
|
||||
options_.real_time_correlative_scan_matcher_options()),
|
||||
ceres_scan_matcher_(options_.ceres_scan_matcher_options()) {}
|
||||
|
||||
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
|
||||
|
||||
const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
|
||||
|
||||
Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
|
||||
|
||||
kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const {
|
||||
return pose_tracker_.get();
|
||||
}
|
||||
|
||||
sensor::LaserFan LocalTrajectoryBuilder::BuildProjectedLaserFan(
|
||||
const transform::Rigid3f& tracking_to_tracking_2d,
|
||||
const sensor::LaserFan3D& laser_fan) const {
|
||||
const sensor::LaserFan projected_fan = sensor::ProjectCroppedLaserFan(
|
||||
sensor::TransformLaserFan3D(laser_fan, tracking_to_tracking_2d),
|
||||
Eigen::Vector3f(-std::numeric_limits<float>::infinity(),
|
||||
-std::numeric_limits<float>::infinity(),
|
||||
options_.horizontal_laser_min_z()),
|
||||
Eigen::Vector3f(std::numeric_limits<float>::infinity(),
|
||||
std::numeric_limits<float>::infinity(),
|
||||
options_.horizontal_laser_max_z()));
|
||||
return sensor::LaserFan{
|
||||
projected_fan.origin,
|
||||
sensor::VoxelFiltered(projected_fan.point_cloud,
|
||||
options_.horizontal_laser_voxel_filter_size()),
|
||||
sensor::VoxelFiltered(projected_fan.missing_echo_point_cloud,
|
||||
options_.horizontal_laser_voxel_filter_size())};
|
||||
}
|
||||
|
||||
void LocalTrajectoryBuilder::ScanMatch(
|
||||
common::Time time, const transform::Rigid3d& pose_prediction,
|
||||
const transform::Rigid3d& tracking_to_tracking_2d,
|
||||
const sensor::LaserFan& laser_fan_in_tracking_2d,
|
||||
transform::Rigid3d* pose_observation,
|
||||
kalman_filter::PoseCovariance* covariance_observation) {
|
||||
const ProbabilityGrid& probability_grid =
|
||||
submaps_.Get(submaps_.matching_index())->probability_grid;
|
||||
transform::Rigid2d pose_prediction_2d =
|
||||
transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse());
|
||||
// The online correlative scan matcher will refine the initial estimate for
|
||||
// the Ceres scan matcher.
|
||||
transform::Rigid2d initial_ceres_pose = pose_prediction_2d;
|
||||
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
|
||||
options_.adaptive_voxel_filter_options());
|
||||
const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d =
|
||||
adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.point_cloud);
|
||||
if (options_.use_online_correlative_scan_matching()) {
|
||||
kalman_filter::Pose2DCovariance unused_covariance_observation;
|
||||
real_time_correlative_scan_matcher_.Match(
|
||||
pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
|
||||
probability_grid, &initial_ceres_pose, &unused_covariance_observation);
|
||||
}
|
||||
|
||||
transform::Rigid2d tracking_2d_to_map;
|
||||
kalman_filter::Pose2DCovariance covariance_observation_2d;
|
||||
ceres::Solver::Summary summary;
|
||||
ceres_scan_matcher_.Match(
|
||||
transform::Project2D(scan_matcher_pose_estimate_ *
|
||||
tracking_to_tracking_2d.inverse()),
|
||||
initial_ceres_pose, filtered_point_cloud_in_tracking_2d, probability_grid,
|
||||
&tracking_2d_to_map, &covariance_observation_2d, &summary);
|
||||
|
||||
CHECK(pose_tracker_ != nullptr);
|
||||
|
||||
*pose_observation = transform::Embed3D(tracking_2d_to_map);
|
||||
// This covariance is used for non-yaw rotation and the fake height of 0.
|
||||
constexpr double kFakePositionCovariance = 1.;
|
||||
constexpr double kFakeOrientationCovariance = 1.;
|
||||
*covariance_observation =
|
||||
kalman_filter::Embed3D(covariance_observation_2d, kFakePositionCovariance,
|
||||
kFakeOrientationCovariance);
|
||||
pose_tracker_->AddPoseObservation(
|
||||
time, (*pose_observation) * tracking_to_tracking_2d,
|
||||
*covariance_observation);
|
||||
}
|
||||
|
||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
||||
LocalTrajectoryBuilder::AddHorizontalLaserFan(
|
||||
const common::Time time, const sensor::LaserFan3D& laser_fan) {
|
||||
// Initialize pose tracker now if we do not ever use an IMU.
|
||||
if (!options_.expect_imu_data()) {
|
||||
InitializePoseTracker(time);
|
||||
}
|
||||
|
||||
if (pose_tracker_ == nullptr) {
|
||||
// Until we've initialized the UKF with our first IMU message, we cannot
|
||||
// compute the orientation of the laser scanner.
|
||||
LOG(INFO) << "PoseTracker not yet initialized.";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
transform::Rigid3d pose_prediction;
|
||||
kalman_filter::PoseCovariance covariance_prediction;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_prediction,
|
||||
&covariance_prediction);
|
||||
|
||||
// Computes the rotation without yaw, as defined by GetYaw().
|
||||
const transform::Rigid3d tracking_to_tracking_2d =
|
||||
transform::Rigid3d::Rotation(
|
||||
Eigen::Quaterniond(Eigen::AngleAxisd(
|
||||
-transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
|
||||
pose_prediction.rotation());
|
||||
|
||||
const sensor::LaserFan laser_fan_in_tracking_2d =
|
||||
BuildProjectedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan);
|
||||
|
||||
if (laser_fan_in_tracking_2d.point_cloud.empty()) {
|
||||
LOG(WARNING) << "Dropped empty horizontal laser point cloud.";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
transform::Rigid3d pose_observation;
|
||||
kalman_filter::PoseCovariance covariance_observation;
|
||||
ScanMatch(time, pose_prediction, tracking_to_tracking_2d,
|
||||
laser_fan_in_tracking_2d, &pose_observation,
|
||||
&covariance_observation);
|
||||
|
||||
kalman_filter::PoseCovariance covariance_estimate;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(
|
||||
time, &scan_matcher_pose_estimate_, &covariance_estimate);
|
||||
|
||||
// Remove the untracked z-component which floats around 0 in the UKF.
|
||||
const auto translation = scan_matcher_pose_estimate_.translation();
|
||||
scan_matcher_pose_estimate_ = transform::Rigid3d(
|
||||
transform::Rigid3d::Vector(translation.x(), translation.y(), 0.),
|
||||
scan_matcher_pose_estimate_.rotation());
|
||||
|
||||
const transform::Rigid3d tracking_2d_to_map =
|
||||
scan_matcher_pose_estimate_ * tracking_to_tracking_2d.inverse();
|
||||
last_pose_estimate_ = {
|
||||
time,
|
||||
{pose_prediction, covariance_prediction},
|
||||
{pose_observation, covariance_observation},
|
||||
{scan_matcher_pose_estimate_, covariance_estimate},
|
||||
scan_matcher_pose_estimate_,
|
||||
sensor::TransformPointCloud(
|
||||
sensor::ToPointCloud(laser_fan_in_tracking_2d.point_cloud),
|
||||
tracking_2d_to_map.cast<float>())};
|
||||
|
||||
const transform::Rigid2d pose_estimate_2d =
|
||||
transform::Project2D(tracking_2d_to_map);
|
||||
if (motion_filter_.IsSimilar(time, transform::Embed3D(pose_estimate_2d))) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
const mapping::Submap* const matching_submap =
|
||||
submaps_.Get(submaps_.matching_index());
|
||||
std::vector<const mapping::Submap*> insertion_submaps;
|
||||
for (int insertion_index : submaps_.insertion_indices()) {
|
||||
insertion_submaps.push_back(submaps_.Get(insertion_index));
|
||||
}
|
||||
submaps_.InsertLaserFan(TransformLaserFan(laser_fan_in_tracking_2d,
|
||||
pose_estimate_2d.cast<float>()));
|
||||
|
||||
return common::make_unique<InsertionResult>(InsertionResult{
|
||||
time, &submaps_, matching_submap, insertion_submaps,
|
||||
tracking_to_tracking_2d, tracking_2d_to_map, laser_fan_in_tracking_2d,
|
||||
pose_estimate_2d, covariance_estimate});
|
||||
}
|
||||
|
||||
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&
|
||||
LocalTrajectoryBuilder::pose_estimate() const {
|
||||
return last_pose_estimate_;
|
||||
}
|
||||
|
||||
void LocalTrajectoryBuilder::AddImuData(
|
||||
const common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) {
|
||||
CHECK(options_.expect_imu_data())
|
||||
<< "An IMU packet was added, but the IMU is not in the "
|
||||
"sensor_configuration.";
|
||||
|
||||
InitializePoseTracker(time);
|
||||
pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
|
||||
pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
|
||||
|
||||
transform::Rigid3d pose_estimate;
|
||||
kalman_filter::PoseCovariance unused_covariance_estimate;
|
||||
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_estimate,
|
||||
&unused_covariance_estimate);
|
||||
|
||||
// Log a warning if the backpack inclination exceeds 20 degrees. In these
|
||||
// cases, it's very likely that 2D SLAM will fail.
|
||||
const Eigen::Vector3d gravity_direction =
|
||||
Eigen::Quaterniond(pose_estimate.rotation()) * Eigen::Vector3d::UnitZ();
|
||||
const double inclination = std::acos(gravity_direction.z());
|
||||
constexpr double kMaxInclination = common::DegToRad(20.);
|
||||
LOG_IF_EVERY_N(WARNING, inclination > kMaxInclination, 1000)
|
||||
<< "Max inclination exceeded: " << common::RadToDeg(inclination) << " > "
|
||||
<< common::RadToDeg(kMaxInclination);
|
||||
}
|
||||
|
||||
void LocalTrajectoryBuilder::AddOdometerPose(
|
||||
const common::Time time, const transform::Rigid3d& pose,
|
||||
const kalman_filter::PoseCovariance& covariance) {
|
||||
if (pose_tracker_ == nullptr) {
|
||||
// Until we've initialized the UKF with our first IMU message, we cannot
|
||||
// process odometry poses.
|
||||
LOG_EVERY_N(INFO, 100) << "PoseTracker not yet initialized.";
|
||||
} else {
|
||||
pose_tracker_->AddOdometerPoseObservation(time, pose, covariance);
|
||||
}
|
||||
}
|
||||
|
||||
void LocalTrajectoryBuilder::InitializePoseTracker(const common::Time time) {
|
||||
if (pose_tracker_ == nullptr) {
|
||||
pose_tracker_ = common::make_unique<kalman_filter::PoseTracker>(
|
||||
options_.pose_tracker_options(),
|
||||
kalman_filter::PoseTracker::ModelFunction::k2D, time);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h"
|
||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
||||
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
|
||||
#include "cartographer/mapping_2d/submaps.h"
|
||||
#include "cartographer/mapping_3d/motion_filter.h"
|
||||
#include "cartographer/sensor/configuration.h"
|
||||
#include "cartographer/sensor/voxel_filter.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
|
||||
common::LuaParameterDictionary* parameter_dictionary);
|
||||
|
||||
// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
|
||||
// closure.
|
||||
class LocalTrajectoryBuilder {
|
||||
public:
|
||||
struct InsertionResult {
|
||||
common::Time time;
|
||||
const mapping::Submaps* submaps;
|
||||
const mapping::Submap* matching_submap;
|
||||
std::vector<const mapping::Submap*> insertion_submaps;
|
||||
transform::Rigid3d tracking_to_tracking_2d;
|
||||
transform::Rigid3d tracking_2d_to_map;
|
||||
sensor::LaserFan laser_fan_in_tracking_2d;
|
||||
transform::Rigid2d pose_estimate_2d;
|
||||
kalman_filter::PoseCovariance covariance_estimate;
|
||||
};
|
||||
|
||||
explicit LocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options);
|
||||
~LocalTrajectoryBuilder();
|
||||
|
||||
LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete;
|
||||
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
|
||||
|
||||
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
|
||||
const;
|
||||
std::unique_ptr<InsertionResult> AddHorizontalLaserFan(
|
||||
common::Time, const sensor::LaserFan3D& laser_fan);
|
||||
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity);
|
||||
void AddOdometerPose(common::Time time, const transform::Rigid3d& pose,
|
||||
const kalman_filter::PoseCovariance& covariance);
|
||||
|
||||
const Submaps* submaps() const;
|
||||
Submaps* submaps();
|
||||
kalman_filter::PoseTracker* pose_tracker() const;
|
||||
|
||||
private:
|
||||
// Transforms 'laser_scan', projects it onto the ground plane,
|
||||
// crops and voxel filters.
|
||||
sensor::LaserFan BuildProjectedLaserFan(
|
||||
const transform::Rigid3f& tracking_to_tracking_2d,
|
||||
const sensor::LaserFan3D& laser_fan) const;
|
||||
|
||||
// Scan match 'laser_fan_in_tracking_2d' and fill in the
|
||||
// 'pose_observation' and 'covariance_observation' with the result.
|
||||
void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
|
||||
const transform::Rigid3d& tracking_to_tracking_2d,
|
||||
const sensor::LaserFan& laser_fan_in_tracking_2d,
|
||||
transform::Rigid3d* pose_observation,
|
||||
kalman_filter::PoseCovariance* covariance_observation);
|
||||
|
||||
// Lazily constructs a PoseTracker.
|
||||
void InitializePoseTracker(common::Time time);
|
||||
|
||||
const proto::LocalTrajectoryBuilderOptions options_;
|
||||
Submaps submaps_;
|
||||
mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_;
|
||||
|
||||
// Pose of the last computed scan match.
|
||||
transform::Rigid3d scan_matcher_pose_estimate_;
|
||||
|
||||
mapping_3d::MotionFilter motion_filter_;
|
||||
scan_matching::RealTimeCorrelativeScanMatcher
|
||||
real_time_correlative_scan_matcher_;
|
||||
scan_matching::CeresScanMatcher ceres_scan_matcher_;
|
||||
|
||||
std::unique_ptr<kalman_filter::PoseTracker> pose_tracker_;
|
||||
};
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_
|
||||
|
|
@ -0,0 +1,193 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_
|
||||
#define CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/mapping/trajectory_node.h"
|
||||
#include "cartographer/mapping_2d/xy_index.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
// Defines the limits of a grid map. This class must remain inlined for
|
||||
// performance reasons.
|
||||
class MapLimits {
|
||||
public:
|
||||
MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits) {
|
||||
SetLimits(resolution, edge_limits);
|
||||
}
|
||||
|
||||
MapLimits(const double resolution, const double max_x, const double max_y,
|
||||
const CellLimits& cell_limits) {
|
||||
SetLimits(resolution, max_x, max_y, cell_limits);
|
||||
}
|
||||
|
||||
MapLimits(const double resolution, const double center_x,
|
||||
const double center_y) {
|
||||
SetLimits(resolution, center_x + 100 * resolution,
|
||||
center_y + 100 * resolution, CellLimits(200, 200));
|
||||
}
|
||||
|
||||
// Returns the cell size in meters. All cells are square and the resolution is
|
||||
// the length of one side.
|
||||
double resolution() const { return resolution_; }
|
||||
|
||||
// Returns the limits of the grid from edge to edge in meters.
|
||||
const Eigen::AlignedBox2d& edge_limits() const { return edge_limits_; }
|
||||
|
||||
// Returns the limits of the grid between the centers of the edge cells in
|
||||
// meters.
|
||||
const Eigen::AlignedBox2d& centered_limits() const {
|
||||
return centered_limits_;
|
||||
}
|
||||
|
||||
// Returns the limits of the grid in number of cells.
|
||||
const CellLimits& cell_limits() const { return cell_limits_; }
|
||||
|
||||
// Returns the index of the cell containing the point ('x', 'y') which may be
|
||||
// outside the map, i.e., negative or too large indices that will return
|
||||
// false for Contains().
|
||||
Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x,
|
||||
const double y) const {
|
||||
return mapping_2d::GetXYIndexOfCellContainingPoint(
|
||||
x, y, centered_limits_.max().x(), centered_limits_.max().y(),
|
||||
resolution_);
|
||||
}
|
||||
|
||||
// Returns true of the ProbabilityGrid contains 'xy_index'.
|
||||
bool Contains(const Eigen::Array2i& xy_index) const {
|
||||
return (Eigen::Array2i(0, 0) <= xy_index).all() &&
|
||||
(xy_index <
|
||||
Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
|
||||
.all();
|
||||
}
|
||||
|
||||
// Computes MapLimits that contain the origin, and all laser rays (both
|
||||
// returns and missing echoes) in the 'trajectory'.
|
||||
static MapLimits ComputeMapLimits(
|
||||
const double resolution,
|
||||
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
|
||||
Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes);
|
||||
// Add some padding to ensure all rays are still contained in the map after
|
||||
// discretization.
|
||||
const float kPadding = 3.f * resolution;
|
||||
bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
|
||||
bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
|
||||
return MapLimits(resolution, bounding_box.cast<double>());
|
||||
}
|
||||
|
||||
static Eigen::AlignedBox2f ComputeMapBoundingBox(
|
||||
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
|
||||
Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
|
||||
for (const auto& node : trajectory_nodes) {
|
||||
const auto& data = *node.constant_data;
|
||||
if (!data.laser_fan_3d.returns.empty()) {
|
||||
const sensor::LaserFan3D laser_fan_3d = sensor::TransformLaserFan3D(
|
||||
Decompress(data.laser_fan_3d), node.pose.cast<float>());
|
||||
bounding_box.extend(laser_fan_3d.origin.head<2>());
|
||||
for (const Eigen::Vector3f& hit : laser_fan_3d.returns) {
|
||||
bounding_box.extend(hit.head<2>());
|
||||
}
|
||||
} else {
|
||||
const sensor::LaserFan laser_fan = sensor::TransformLaserFan(
|
||||
data.laser_fan, transform::Project2D(node.pose).cast<float>());
|
||||
bounding_box.extend(laser_fan.origin);
|
||||
for (const Eigen::Vector2f& hit : laser_fan.point_cloud) {
|
||||
bounding_box.extend(hit);
|
||||
}
|
||||
for (const Eigen::Vector2f& miss : laser_fan.missing_echo_point_cloud) {
|
||||
bounding_box.extend(miss);
|
||||
}
|
||||
}
|
||||
}
|
||||
return bounding_box;
|
||||
}
|
||||
|
||||
private:
|
||||
// Sets the cell size to the specified resolution in meters and the limits of
|
||||
// the grid to the specified bounding box in meters.
|
||||
void SetLimits(double resolution, const Eigen::AlignedBox2d& limits) {
|
||||
CHECK(!limits.isEmpty());
|
||||
const int num_x_cells =
|
||||
common::RoundToInt((mapping_2d::Center(limits.max().y(), resolution) -
|
||||
mapping_2d::Center(limits.min().y(), resolution)) /
|
||||
resolution) +
|
||||
1;
|
||||
const int num_y_cells =
|
||||
common::RoundToInt((mapping_2d::Center(limits.max().x(), resolution) -
|
||||
mapping_2d::Center(limits.min().x(), resolution)) /
|
||||
resolution) +
|
||||
1;
|
||||
SetLimits(resolution, limits.max().x(), limits.max().y(),
|
||||
CellLimits(num_x_cells, num_y_cells));
|
||||
}
|
||||
|
||||
// Sets the cell size to the specified resolution in meters and the limits of
|
||||
// the grid to the specified bounding box.
|
||||
//
|
||||
// Note that implementing this in terms of the previous SetLimits method
|
||||
// results in unnecessary (and expensive?) calls to common::RoundToInt.
|
||||
//
|
||||
// TODO(whess): Measure whether it really is still too slow. Otherwise,
|
||||
// simplify.
|
||||
void SetLimits(double resolution, double max_x, double max_y,
|
||||
const CellLimits& limits) {
|
||||
CHECK_GT(resolution, 0.);
|
||||
CHECK_GT(limits.num_x_cells, 0.);
|
||||
CHECK_GT(limits.num_y_cells, 0.);
|
||||
|
||||
resolution_ = resolution;
|
||||
cell_limits_ = limits;
|
||||
centered_limits_.max().x() = Center(max_x, resolution);
|
||||
centered_limits_.max().y() = Center(max_y, resolution);
|
||||
centered_limits_.min().x() = centered_limits_.max().x() -
|
||||
resolution_ * (cell_limits_.num_y_cells - 1);
|
||||
centered_limits_.min().y() = centered_limits_.max().y() -
|
||||
resolution_ * (cell_limits_.num_x_cells - 1);
|
||||
UpdateEdgeLimits();
|
||||
}
|
||||
|
||||
// Updates the edge limits from the previously calculated centered limits.
|
||||
void UpdateEdgeLimits() {
|
||||
const double half_resolution = resolution_ / 2.;
|
||||
edge_limits_.min().x() = centered_limits_.min().x() - half_resolution;
|
||||
edge_limits_.min().y() = centered_limits_.min().y() - half_resolution;
|
||||
edge_limits_.max().x() = centered_limits_.max().x() + half_resolution;
|
||||
edge_limits_.max().y() = centered_limits_.max().y() + half_resolution;
|
||||
}
|
||||
|
||||
double resolution_;
|
||||
Eigen::AlignedBox2d edge_limits_;
|
||||
Eigen::AlignedBox2d centered_limits_;
|
||||
CellLimits cell_limits_;
|
||||
};
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,197 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
|
||||
#define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping_2d/map_limits.h"
|
||||
#include "cartographer/mapping_2d/xy_index.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
// Represents a 2D grid of probabilities.
|
||||
class ProbabilityGrid {
|
||||
public:
|
||||
explicit ProbabilityGrid(const MapLimits& limits)
|
||||
: limits_(limits),
|
||||
cells_(limits_.cell_limits().num_x_cells *
|
||||
limits_.cell_limits().num_y_cells,
|
||||
mapping::kUnknownProbabilityValue),
|
||||
max_x_(0),
|
||||
max_y_(0),
|
||||
min_x_(limits_.cell_limits().num_x_cells - 1),
|
||||
min_y_(limits_.cell_limits().num_y_cells - 1) {}
|
||||
|
||||
// Returns the limits of this ProbabilityGrid.
|
||||
const MapLimits& limits() const { return limits_; }
|
||||
|
||||
// Starts the next update sequence.
|
||||
void StartUpdate() {
|
||||
while (!update_indices_.empty()) {
|
||||
DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
|
||||
cells_[update_indices_.back()] -= mapping::kUpdateMarker;
|
||||
update_indices_.pop_back();
|
||||
}
|
||||
}
|
||||
|
||||
// Sets the probability of the cell at 'xy_index' to the given 'probability'.
|
||||
// Only allowed if the cell was unknown before.
|
||||
void SetProbability(const Eigen::Array2i& xy_index, const float probability) {
|
||||
uint16& cell = cells_[GetIndexOfCell(xy_index)];
|
||||
CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
|
||||
cell = mapping::ProbabilityToValue(probability);
|
||||
UpdateBounds(xy_index);
|
||||
}
|
||||
|
||||
// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
|
||||
// to the probability of the cell at 'xy_index' if the cell has not already
|
||||
// been updated. Multiple updates of the same cell will be ignored until
|
||||
// StartUpdate() is called. Returns true if the cell was updated.
|
||||
//
|
||||
// If this is the first call to ApplyOdds() for the specified cell, its value
|
||||
// will be set to probability corresponding to 'odds'.
|
||||
bool ApplyLookupTable(const Eigen::Array2i& xy_index,
|
||||
const std::vector<uint16>& table) {
|
||||
DCHECK_EQ(table.size(), mapping::kUpdateMarker);
|
||||
const int cell_index = GetIndexOfCell(xy_index);
|
||||
uint16& cell = cells_[cell_index];
|
||||
if (cell >= mapping::kUpdateMarker) {
|
||||
return false;
|
||||
}
|
||||
update_indices_.push_back(cell_index);
|
||||
cell = table[cell];
|
||||
DCHECK_GE(cell, mapping::kUpdateMarker);
|
||||
UpdateBounds(xy_index);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Returns the probability of the cell with 'xy_index'.
|
||||
float GetProbability(const Eigen::Array2i& xy_index) const {
|
||||
if (limits_.Contains(xy_index)) {
|
||||
return mapping::ValueToProbability(cells_[GetIndexOfCell(xy_index)]);
|
||||
}
|
||||
return mapping::kMinProbability;
|
||||
}
|
||||
|
||||
// Returns the probability of the cell containing the point ('x', 'y').
|
||||
float GetProbability(const double x, const double y) const {
|
||||
return GetProbability(limits_.GetXYIndexOfCellContainingPoint(x, y));
|
||||
}
|
||||
|
||||
// Returns true if the probability at the specified index is known.
|
||||
bool IsKnown(const Eigen::Array2i& xy_index) const {
|
||||
return limits_.Contains(xy_index) &&
|
||||
cells_[GetIndexOfCell(xy_index)] !=
|
||||
mapping::kUnknownProbabilityValue;
|
||||
}
|
||||
|
||||
// Returns the center of the cell at 'xy_index'.
|
||||
void GetCenterOfCellWithXYIndex(const Eigen::Array2i& xy_index,
|
||||
double* const x, double* const y) const {
|
||||
*CHECK_NOTNULL(x) = limits_.edge_limits().max().x() -
|
||||
(xy_index.y() + 0.5) * limits_.resolution();
|
||||
*CHECK_NOTNULL(y) = limits_.edge_limits().max().y() -
|
||||
(xy_index.x() + 0.5) * limits_.resolution();
|
||||
}
|
||||
|
||||
// Fills in 'offset' and 'limits' to define a subregion of that contains all
|
||||
// known cells.
|
||||
void ComputeCroppedLimits(Eigen::Array2i* const offset,
|
||||
CellLimits* const limits) const {
|
||||
*offset = Eigen::Array2i(min_x_, min_y_);
|
||||
*limits = CellLimits(std::max(max_x_, min_x_) - min_x_ + 1,
|
||||
std::max(max_y_, min_y_) - min_y_ + 1);
|
||||
}
|
||||
|
||||
// Grows the map as necessary to include 'x' and 'y'. This changes the meaning
|
||||
// of these coordinates going forward. This method must be called immediately
|
||||
// after 'StartUpdate', before any calls to 'ApplyLookupTable'.
|
||||
void GrowLimits(const double x, const double y) {
|
||||
CHECK(update_indices_.empty());
|
||||
while (!limits_.Contains(limits_.GetXYIndexOfCellContainingPoint(x, y))) {
|
||||
const int x_offset = limits_.cell_limits().num_x_cells / 2;
|
||||
const int y_offset = limits_.cell_limits().num_y_cells / 2;
|
||||
const MapLimits new_limits(
|
||||
limits_.resolution(),
|
||||
limits_.centered_limits().max().x() + y_offset * limits_.resolution(),
|
||||
limits_.centered_limits().max().y() + x_offset * limits_.resolution(),
|
||||
CellLimits(2 * limits_.cell_limits().num_x_cells,
|
||||
2 * limits_.cell_limits().num_y_cells));
|
||||
const int stride = new_limits.cell_limits().num_x_cells;
|
||||
const int offset = x_offset + stride * y_offset;
|
||||
const int new_size = new_limits.cell_limits().num_x_cells *
|
||||
new_limits.cell_limits().num_y_cells;
|
||||
std::vector<uint16> new_cells(new_size,
|
||||
mapping::kUnknownProbabilityValue);
|
||||
for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
|
||||
for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
|
||||
new_cells[offset + j + i * stride] =
|
||||
cells_[j + i * limits_.cell_limits().num_x_cells];
|
||||
}
|
||||
}
|
||||
cells_ = new_cells;
|
||||
limits_ = new_limits;
|
||||
min_x_ += x_offset;
|
||||
min_y_ += y_offset;
|
||||
max_x_ += x_offset;
|
||||
max_y_ += y_offset;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
// Returns the index of the cell at 'xy_index'.
|
||||
int GetIndexOfCell(const Eigen::Array2i& xy_index) const {
|
||||
CHECK(limits_.Contains(xy_index)) << xy_index;
|
||||
return mapping_2d::GetIndexOfCell(xy_index,
|
||||
limits_.cell_limits().num_x_cells);
|
||||
}
|
||||
|
||||
void UpdateBounds(const Eigen::Array2i& xy_index) {
|
||||
min_x_ = std::min(min_x_, xy_index.x());
|
||||
min_y_ = std::min(min_y_, xy_index.y());
|
||||
max_x_ = std::max(max_x_, xy_index.x());
|
||||
max_y_ = std::max(max_y_, xy_index.y());
|
||||
}
|
||||
|
||||
MapLimits limits_;
|
||||
std::vector<uint16> cells_; // Highest bit is update marker.
|
||||
std::vector<int> update_indices_;
|
||||
|
||||
// Minimum and maximum cell coordinates of know cells to efficiently compute
|
||||
// cropping limits.
|
||||
int max_x_;
|
||||
int max_y_;
|
||||
int min_x_;
|
||||
int min_y_;
|
||||
};
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
|
||||
|
|
@ -0,0 +1,190 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
|
||||
#include <random>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
namespace {
|
||||
|
||||
TEST(ProbabilityGridTest, ApplyOdds) {
|
||||
ProbabilityGrid probability_grid(MapLimits(1., 0.5, 0.5, CellLimits(2, 2)));
|
||||
const MapLimits& limits = probability_grid.limits();
|
||||
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 1)));
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 0)));
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 1)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 0)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 1)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 0)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 1)));
|
||||
|
||||
probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5);
|
||||
|
||||
probability_grid.StartUpdate();
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 0),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
|
||||
EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5);
|
||||
|
||||
probability_grid.StartUpdate();
|
||||
probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5);
|
||||
|
||||
probability_grid.StartUpdate();
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(0, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1)));
|
||||
EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5);
|
||||
|
||||
// Tests adding odds to an unknown cell.
|
||||
probability_grid.StartUpdate();
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42)));
|
||||
EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
|
||||
1e-4);
|
||||
|
||||
// Tests that further updates are ignored if StartUpdate() isn't called.
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
|
||||
EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
|
||||
1e-4);
|
||||
probability_grid.StartUpdate();
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
|
||||
EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42);
|
||||
}
|
||||
|
||||
TEST(ProbabilityGridTest, GetProbability) {
|
||||
ProbabilityGrid probability_grid(
|
||||
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 0.5),
|
||||
Eigen::Vector2d(0.5, 1.5))));
|
||||
|
||||
const MapLimits& limits = probability_grid.limits();
|
||||
const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
|
||||
EXPECT_EQ(-1., edge_limits.min().x());
|
||||
EXPECT_EQ(0., edge_limits.min().y());
|
||||
EXPECT_EQ(1., edge_limits.max().x());
|
||||
EXPECT_EQ(2., edge_limits.max().y());
|
||||
|
||||
const CellLimits& cell_limits = limits.cell_limits();
|
||||
ASSERT_EQ(2, cell_limits.num_x_cells);
|
||||
ASSERT_EQ(2, cell_limits.num_y_cells);
|
||||
|
||||
probability_grid.StartUpdate();
|
||||
probability_grid.SetProbability(
|
||||
limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5),
|
||||
mapping::kMaxProbability);
|
||||
EXPECT_NEAR(probability_grid.GetProbability(-0.5, 0.5),
|
||||
mapping::kMaxProbability, 1e-6);
|
||||
for (const Eigen::Array2i& xy_index :
|
||||
{limits.GetXYIndexOfCellContainingPoint(-0.5, 1.5),
|
||||
limits.GetXYIndexOfCellContainingPoint(0.5, 0.5),
|
||||
limits.GetXYIndexOfCellContainingPoint(0.5, 1.5)}) {
|
||||
EXPECT_TRUE(limits.Contains(xy_index));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(xy_index));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
|
||||
ProbabilityGrid probability_grid(MapLimits(2., 7, 13, CellLimits(14, 8)));
|
||||
|
||||
const MapLimits& limits = probability_grid.limits();
|
||||
const CellLimits& cell_limits = limits.cell_limits();
|
||||
ASSERT_EQ(14, cell_limits.num_x_cells);
|
||||
ASSERT_EQ(8, cell_limits.num_y_cells);
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(0, 0) == limits.GetXYIndexOfCellContainingPoint(7, 13))
|
||||
.all());
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13))
|
||||
.all());
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13))
|
||||
.all());
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13))
|
||||
.all());
|
||||
|
||||
// Check around the origin.
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, 0.5))
|
||||
.all());
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(1.5, 1.5))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(7, 3) ==
|
||||
limits.GetXYIndexOfCellContainingPoint(0.5, -0.5))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(6, 4) ==
|
||||
limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(7, 4) ==
|
||||
limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5))
|
||||
.all());
|
||||
}
|
||||
|
||||
TEST(ProbabilityGridTest, GetCenterOfCell) {
|
||||
ProbabilityGrid probability_grid(MapLimits(
|
||||
2.,
|
||||
Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.), Eigen::Vector2d(-0., 6.))));
|
||||
|
||||
// Limits are on the edge of a cell. Make sure that the grid grows (in
|
||||
// positive direction).
|
||||
const MapLimits& limits = probability_grid.limits();
|
||||
const CellLimits& cell_limits = limits.cell_limits();
|
||||
EXPECT_EQ(4, cell_limits.num_x_cells);
|
||||
EXPECT_EQ(3, cell_limits.num_y_cells);
|
||||
|
||||
const Eigen::Array2i xy_index(3, 2);
|
||||
double x, y;
|
||||
probability_grid.GetCenterOfCellWithXYIndex(xy_index, &x, &y);
|
||||
EXPECT_NEAR(-3., x, 1e-6);
|
||||
EXPECT_NEAR(1., y, 1e-6);
|
||||
EXPECT_TRUE((xy_index == limits.GetXYIndexOfCellContainingPoint(x, y)).all());
|
||||
}
|
||||
|
||||
TEST(ProbabilityGridTest, CorrectCropping) {
|
||||
// Create a probability grid with random values.
|
||||
std::mt19937 rng(42);
|
||||
std::uniform_real_distribution<float> value_distribution(
|
||||
mapping::kMinProbability, mapping::kMaxProbability);
|
||||
ProbabilityGrid probability_grid(
|
||||
MapLimits(0.05, 10., 10., CellLimits(400, 400)));
|
||||
probability_grid.StartUpdate();
|
||||
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
|
||||
probability_grid.limits().cell_limits(), Eigen::Array2i(100, 100),
|
||||
Eigen::Array2i(299, 299))) {
|
||||
probability_grid.SetProbability(xy_index, value_distribution(rng));
|
||||
}
|
||||
Eigen::Array2i offset;
|
||||
CellLimits limits;
|
||||
probability_grid.ComputeCroppedLimits(&offset, &limits);
|
||||
EXPECT_TRUE((offset == Eigen::Array2i(100, 100)).all());
|
||||
EXPECT_EQ(limits.num_x_cells, 200);
|
||||
EXPECT_EQ(limits.num_y_cells, 200);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,187 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_2d/ray_casting.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
namespace {
|
||||
|
||||
// Factor for subpixel accuracy of start and end point.
|
||||
constexpr int kSubpixelScale = 1000;
|
||||
|
||||
// We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin'
|
||||
// and 'end' are coordinates at subpixel precision. We compute all pixels in
|
||||
// which some part of the line segment connecting 'begin' and 'end' lies.
|
||||
void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
|
||||
const std::function<void(const Eigen::Array2i&)>& visitor) {
|
||||
// For simplicity, we order 'begin' and 'end' by their x coordinate.
|
||||
if (begin.x() > end.x()) {
|
||||
CastRay(end, begin, visitor);
|
||||
return;
|
||||
}
|
||||
|
||||
CHECK_GE(begin.x(), 0);
|
||||
CHECK_GE(begin.y(), 0);
|
||||
CHECK_GE(end.y(), 0);
|
||||
|
||||
// Special case: We have to draw a vertical line in full pixels, as 'begin'
|
||||
// and 'end' have the same full pixel x coordinate.
|
||||
if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
|
||||
Eigen::Array2i current(begin.x() / kSubpixelScale,
|
||||
std::min(begin.y(), end.y()) / kSubpixelScale);
|
||||
const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
|
||||
for (; current.y() <= end_y; ++current.y()) {
|
||||
visitor(current);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
const int64 dx = end.x() - begin.x();
|
||||
const int64 dy = end.y() - begin.y();
|
||||
const int64 denominator = 2 * kSubpixelScale * dx;
|
||||
|
||||
// The current full pixel coordinates. We begin at 'begin'.
|
||||
Eigen::Array2i current = begin / kSubpixelScale;
|
||||
|
||||
// To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
|
||||
// the denominator.
|
||||
// +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
|
||||
// | | | |
|
||||
// +-+-+-+
|
||||
// | | | |
|
||||
// +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
|
||||
// | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
|
||||
// +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)
|
||||
|
||||
// The center of the subpixel part of 'begin.y()' assuming the
|
||||
// 'denominator', i.e., sub_y / denominator is in (0, 1).
|
||||
int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;
|
||||
|
||||
// The distance from the from 'begin' to the right pixel border, to be divided
|
||||
// by 2 * 'kSubpixelScale'.
|
||||
const int first_pixel =
|
||||
2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
|
||||
// The same from the left pixel border to 'end'.
|
||||
const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;
|
||||
|
||||
// The full pixel x coordinate of 'end'.
|
||||
const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;
|
||||
|
||||
// Move from 'begin' to the next pixel border to the right.
|
||||
sub_y += dy * first_pixel;
|
||||
if (dy > 0) {
|
||||
while (true) {
|
||||
visitor(current);
|
||||
while (sub_y > denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
visitor(current);
|
||||
}
|
||||
++current.x();
|
||||
if (sub_y == denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
}
|
||||
if (current.x() == end_x) {
|
||||
break;
|
||||
}
|
||||
// Move from one pixel border to the next.
|
||||
sub_y += dy * 2 * kSubpixelScale;
|
||||
}
|
||||
// Move from the pixel border on the right to 'end'.
|
||||
sub_y += dy * last_pixel;
|
||||
visitor(current);
|
||||
while (sub_y > denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
visitor(current);
|
||||
}
|
||||
CHECK_NE(sub_y, denominator);
|
||||
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
|
||||
return;
|
||||
}
|
||||
|
||||
// Same for lines non-ascending in y coordinates.
|
||||
while (true) {
|
||||
visitor(current);
|
||||
while (sub_y < 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
visitor(current);
|
||||
}
|
||||
++current.x();
|
||||
if (sub_y == 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
}
|
||||
if (current.x() == end_x) {
|
||||
break;
|
||||
}
|
||||
sub_y += dy * 2 * kSubpixelScale;
|
||||
}
|
||||
sub_y += dy * last_pixel;
|
||||
visitor(current);
|
||||
while (sub_y < 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
visitor(current);
|
||||
}
|
||||
CHECK_NE(sub_y, 0);
|
||||
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
|
||||
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
|
||||
const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
|
||||
const double resolution = limits.resolution() / kSubpixelScale;
|
||||
const Eigen::Vector2d superscaled_centered_max =
|
||||
limits.edge_limits().max() - resolution / 2. * Eigen::Vector2d::Ones();
|
||||
const Eigen::Array2i begin = GetXYIndexOfCellContainingPoint(
|
||||
laser_fan.origin.x(), laser_fan.origin.y(), superscaled_centered_max.x(),
|
||||
superscaled_centered_max.y(), resolution);
|
||||
|
||||
// Compute and add the end points.
|
||||
std::vector<Eigen::Array2i> ends;
|
||||
ends.reserve(laser_fan.point_cloud.size());
|
||||
for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
|
||||
ends.push_back(GetXYIndexOfCellContainingPoint(
|
||||
laser_return.x(), laser_return.y(), superscaled_centered_max.x(),
|
||||
superscaled_centered_max.y(), resolution));
|
||||
hit_visitor(ends.back() / kSubpixelScale);
|
||||
}
|
||||
|
||||
// Now add the misses.
|
||||
for (const Eigen::Array2i& end : ends) {
|
||||
CastRay(begin, end, miss_visitor);
|
||||
}
|
||||
|
||||
// Finally, compute and add empty rays based on missing echos in the scan.
|
||||
for (const Eigen::Vector2f& missing_echo :
|
||||
laser_fan.missing_echo_point_cloud) {
|
||||
CastRay(begin, GetXYIndexOfCellContainingPoint(
|
||||
missing_echo.x(), missing_echo.y(),
|
||||
superscaled_centered_max.x(),
|
||||
superscaled_centered_max.y(), resolution),
|
||||
miss_visitor);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_
|
||||
#define CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include "cartographer/mapping_2d/map_limits.h"
|
||||
#include "cartographer/mapping_2d/xy_index.h"
|
||||
#include "cartographer/sensor/laser.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
// For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the
|
||||
// appropriate cells. Hits are handled before misses.
|
||||
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
|
||||
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
|
||||
const std::function<void(const Eigen::Array2i&)>& miss_visitor);
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_
|
||||
|
|
@ -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
|
||||
)
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue